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ABSTRACT 


A  program  was  written  to  simulate  the  three  axis  attitude  control  system 
of  LES-8/9.  The  underlying  theory  to  the  computer  simulation  and  de¬ 
tailed  outlines  of  each  of  the  subroutines  involved  in  the  complete  program 
are  described. 

Whenever  feasible,  the  simulation  was  written  to  duplicate  as  closely  as 
possible  the  logic  and  signal  flow  of  the  actual  digital  attitude  control  sys¬ 
tem.  Each  subroutine  was  thoroughly  verified  as  accurate  by  independent 
and  integral  system  operation,  as  well  as  by  theoretical  estimates,  analog 
computer  simulations  and  actual  experimental  data. 

A  substantial  compilation  of  data  from  this  working  program  was  catalogued 
and  analyzed  for  attitude  control  system  evaluation  and  optimization.  This 
program  also  proved  itself  to  be  invaluable  in  the  analysis  of  stability  and 
performance  of  the  complete  attitude  control  system. 


Accepted  for  the  Air  Force 

Joseph  R.  Waterman,  Lt.  Col.,  USAF 

Chief,  Lincoln  Laboratory  Project  Office 


CONTENTS 


Abstract  iii 

Nomenclature  vi 

I.  Introduction  1 

II.  System  Equations  1 

A.  Euler  Equations  of  Motion  1 

R.  Euler  Angle  Transformations  2 

C.  Earth  Sensor  Measured  Angles  2 

D.  Earth  Sensor  Simulation  2 

III,  Attitude  Control  Laws  3 

A.  Pitch  Axis  Control  Modes  3 

R.  Pitch  Axis  Coarse  Momentum  Control  7 

C.  Roll  Axis  Control  Modes  9 

IV.  Description  of  Simulation  1  1 

A.  Main  Program  of  the  Simulation  1  3 

R.  Major  Subroutines  15 

C.  Output  Format  and  Plotting  Subroutines  2  3 

V.  Use  of  the  Program  25 

A.  Input  Variables  2  5 

R.  Output  Data  27 

Appendix  29 


v 


NOMENCLATURE 


zr  Z2’  Z3 

principal  axes  of  satellite  body  designated  pitch,  roll,  yaw 
axes,  respectively 

Yi 

Euler  angle  representing  pitch  axis  pointing  error 

Y2 

Euler  angle  representing  roll  axis  pointing  error 

Y3 

Euler  angle  representing  yaw  axis  pointing  error 

Y4 

momentum  wheel  gimbal  angle 

Y5 

angular  momentum  stored  in  reaction  wheel  (wheel  speed) 

Y6 

rate  about  pitch  axis 

Y7 

rate  about  roll  axis 

Y8 

rate  about  yaw  axis 

K 

qr 

S1 

quantization  scale  factor  for  roll  axis  control  law  (ft- lbs/bit) 

sensor  measured  angle  representing  pitch  axis  pointing  error 

S2 

sensor  measured  angle  representing  roll  axis  pointing  error 

XJ.,  XJ_,  XJ, 

1  L  3 

satellite  principal  inertias  about  pitch,  roll,  yaw  axes, 
respectively 

XJ4 

inertia  of  reaction  wheel  rotor  about  spin  axis 

M2,  M3 

misalignment  (offset)  angles  between  gimbal  axis  and  satellite 
principal  axis 

1RNR 

roll  axis  control  law  input  to  gimbal  power  amplifier 

IRNP 

pitch  axis  control  law  input  to  momentum  wheel  speed  con¬ 
trol  logic 

LI,  L2 ,  L3 

pulsed  plasma  thruster  control  torques  applied  along  the  axes 
Z^,  Z^,  Zy  respectively 

LSB 

momentum  wheel  speed  period  quantization  in  parts  per 
second 

XD1 

gimbal  damping  coefficient 

XD2 

gimbal  flex  pivot  spring  constant 

T 131 ,  TD2 ,  TD3 

external  disturbance  torques  applied  along  the  Z^,  Z^,  Z^ 
axes,  respectively 

SIGMA 

standard  deviation  (rms  value)  of  random  noise  generated 
by  earth  sensor 

XP1,  XP2,  NP 

XRl ,  XR2,  NR 

Tl,  T2 

N 

pitch  axis  control  law  parameters 

roll  axis  control  law  parameters 

reaction  control  torque  of  momentum  wheel  about  its  spin  axis 
in  the  on-off  state,  respectively 

update  period  or  interval  between  sampling  times 

vi 


LES-8/9  ATTITUDE  CONTROL  SYSTEM  SIMULATION 


l.  INTRODUCTION 

The  numerical  simulation  of  the  motion  of  a  rigid  body  satellite  containing  a  single  gimbaled 
momentum  wheel  about  its  center  of  gravity  is  described.  Also  included  is  a  simulation  of  the 
digital  logic  flow  of  the  various  attitude  control  laws.  IR  earth  sensors  and  pulsed  plasma  thrusters. 

A  brief  description  of  the  satellite  equations  of  motion  and  kinematics  is  included  in  Sec.  11. 

An  Adams- Moulton,  Runge-Kutta  integration  subroutine1  was  used  to  solve  the  basic  body  dy¬ 
namic  equations.  This  simulation  is  intended  for  the  analysis  of  relatively  short  real  time  atti¬ 
tude  motion  of  the  satellite  on  the  order  of  a  few  minutes.  Required  integration  time  is  large 
enough  in  this  program  so  that  the  long  term  steady  state  response  to  very  low  frequency  disturb¬ 
ances  such  as  solar  pressure  torques  are  more  economically  determined  in  a  separate  simula¬ 
tion  not  included  here.  The  exact  kinematics  are  included  so  that  both  small  and  large  angle 
attitude  motion  can  be  simulated  with  this  program. 

The  various  parts  of  the  simulation  are  broken  into  13  subroutines,  each  of  which  is  briefly 
described  in  Sec.  IV  of  this  report.  Required  input  data  and  typical  output  data  for  the  program 
are  given  in  Sec.  V.  A  listing  of  the  program  is  given  in  the  Appendix.  The  attitude  control  laws 
for  each  operating  mode  of  the  pitch  and  roll  axis  systems  are  described  in  Sec.  III. 


II.  SYSTEM  EQUATIONS 

The  Euler  equations  of  motion  for  a  rigid  body  satellite  containing  a  single  gimbaled  momen¬ 
tum  wheel  are  given  in  Ref.  2,  along  with  the  assumptions  used  in  the  derivation.  These  equations 
are  solved  in  the  numerical  simulation  to  provide  the  satellite  body  rates.  The  body  rates  are 
then  converted  to  Euler  angle  rates  referenced  to  an  orbital  reference  frame.  The  Euler  angle 
rates  are  then  integrated  to  provide  an  inertial  attitude  reference  for  the  satellite.  The  Euler 
angles  are  finally  converted  to  earth  sensor  measured  angles  which  are  the  true  inputs  to  the  atti¬ 
tude  control  system. 

A.  Euler  Equations  of  Motion 

V4  =  -  XIT  {<XD2)  Y4  +  Y5  [Y8  +  <Y4  +  M2>  (Y6  “  M3Y7>D  “  K  <1RNR)  (1  ) 

1  4 


Y5  = 

T 

M 

(2) 

Y6 

1 

XJi 

(E 

+  TDj  +  <XJ23)  Y7Y8  +  <Y4  +  M2)  Y5(Y4  +  Y?>- 

TM  M3CY8' 

(3) 

Y7 

1 

XJ2 

[L2 

+  ™2  +  <XJ31>  Y6Y8  -  (Y4  +  M2>  Y5(Y6  +  M3Y4 

>  +  M3TM-  W 

(4) 

^8  " 

1 

xj2 

[l3 

+  td3  -  <XJ21)  Y6Y7  +  Y5(Y4  +  Y7)  +  <Y4  +  m2) 

TM  +  M3Y5Y6!  ' 

(5) 

Conversion  from  a  satellite  reference  frame  to  an  orbit  reference  frame  is  accomplished  using 
the  standard  Euler  angle  transformation  with  Y^ ,  Y^,  Y^  taken  about  the  axes  Z^.  Z^  in  this 
order,  respectively. 
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B.  Euler  Angle  Transformations'* 

Yj  =  [Y^  cos  Y^  -  Y^  sin  Y^]/cos  Y^  -  (6 ) 

Y2  =  sinY^  +  Y^  cos  Y^  (7) 

Y^  =  Yg  —  tan  Y2  [  Y^  cos  Y^  -  Y^  sin  Y^]  .  (8 ) 

C.  Earth  Sensor  Measured  Angles 

—  1 

S.  =  tan  [sinY.  cos  Y,  +  cos  Y.  sinY,  sin  1  /cos  Y  .  cos  Y0  (9) 

l  li  13  2  12 

S2  =  tan  [cos  Y^  sinY2  cos  Y^  -  sinY^  sinY^l/cosY^  cos  Y^  •  (10) 


D.  Earth  Sensor  Simulation 

Relating  the  earth  pitch  axis  sensor  measured  angle  S^  to  the  output  { ISAMP)  of  the  pitch 
sensor  at  sampling  intervals  of  (IS)  seconds  yields 

ISAM  P(  IS)  =  [S^ /DELTAR  +  RMS]  ,  (11) 


where 


DELTAR  =  least  significant  bit  of  sensor  quantized  output  (rad/bit) 

RMS  =  quantized  Gaussian  distributed  random  noise  generated 
by  sensor  with  zero  mean  and  standard  deviation  SIGMA 

IS  =  sampling  interval  =  1,2,...  16. 

At  periods  of  16  sampling  intervals  the  value  of  IS  is  reset  to  zero  and  the  stored  values  of 
ISAMP(IS)  are  averaged  to  yield  the  sensor  output  ISP. 

16 

ISP  =  -  Yg  y  ISAMP(l)  .  (12) 

1=1 


The  value  of  ISP  is  updated  once  for  every  16  samples  of  ISAMP.  Since  ISAMP  is  sampled  once 
every  DELTAT  seconds,  the  value  of  ISP  is  updated  once  every  N  seconds,  where  N  =  ( 1 6 ) 
DELTAT. 

The  roll  sensor  is  simulated  identically  to  the  pitch  sensor.  Repeating  Eqs.(ll)  and  (12) 
with  roll  sensor  variables  yields 

ISAMR(IS)  =  [S2 /DELTAR  +  RMSR]  (13) 


16 

ISR  =  jl  Z  ISAMR(I)  .  (14) 

1=1 

Thus,  the  quantized  output  of  the  pitch  and  roll  earth  sensors  are  named  ISP  and  ISR,  respectively. 

The  linear  field  of  view  limitation  on  both  sensors  is  included  in  the  simulation.  The  total 
field  of  view  is  not  terminated  abruptly  at  the  end  of  the  linear  range,  but  extends  in  a  piecewise 
linear  symmetrical  conf iguration  called  a  "foldover"  characteristic.  This  characteristic  effec¬ 
tively  doubles  the  acquisition  field  of  view  of  the  control  system.  Also  included  here  is  the  effect 
of  roll  attitude  error  on  the  pitch  sensor  field  of  view  and  vice  versa. 
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111.  ATTITUDE  CONTROL  LAWS 


The  attitude  control  laws  for  the  pitch  and  roll  axis  systems  are  broken  into  two  categories 
(a)  momentum  exchange  and  (b)  momentum  expulsion.  In  this  program  there  are  several  control 
modes  simulated  for  both  pitch  and  roll  axis  control.  A  separate  control  law  is  described  for 
each  operating  mode  in  the  attitude  control  system. 

A.  Pitch  Axis  Control  Modes 

There  are  5  operating  modes  available  for  controlling  the  satellite  attitude  about  the  pitch 
axis.  Four  modes  utilize  momentum  exchange  capability  of  the  reaction  wheel  and  one  uses  the 
pulsed  plasma  thruster  for  direct  momentum  expulsion  attitude  control.  The  control  laws  utilized 
in  these  5  operating  modes  are  described. 

1.  Momentum  Wheel  Speed  Control 

The  control  laws  relating  to  the  motor  torque  T^  and  momentum  to  the  wheel  speed  ref¬ 
erence  input  1RNP  are  given  in  this  section.  The  actual  relative  wheel  speed  period  Tp  is 
simulated  from  the  equation 

Tp  =  2r/[V5/XJ4  -  Y6  +  Y4Y8j  .  (15) 

The  control  system  determines  the  wheel  speed  period  Tp  by  measuring  the  interval  between 
8  successive  tachometer  pulses.  The  value  of  Tp  is  quantized  into  LSB  bits  per  second  and  is 
updated  at  the  rate  of  once  per  period.  The  digital  number  used  to  represent  Tp  is  given  by 
IPNP,  where 

1PNP  =  Tp(LSB)  bits  .  (  1 6 ) 

The  value  of  IPNP  is  compared  once  per  period  with  the  wheel  speed  command  1RNP  to  determine 
the  error  in  the  loop. 

1  ERROR  =  (IPNP  —  1RNP)  .  (17) 

The  momentum  wheel  speed  is  binary  controlled  by  either  applying  a  constant  reference  voltage 
or  zero  to  the  motor  windings  according  to  the  control  law: 


If 

(ERROR  <  0) 

T  =  -  T 

1  M  2 

If 

(ERROR  >0) 

T  -  T 

1  M  1  1 

(18) 

This  binary  control  loop  is  updated  once  per  period. 

The  reaction  torque  values  Tp  T^  used  in  the  simulation  closely  approximate  the  actual 
nonlinear  wheel  control  torques.  This  is  accomplished  by  least- squares,  fitting  a  first  degree 
function  to  a  set  of  experimentally  determined  torque  values  from  the  actual  wheel. 

V5 

Tj  =  A^  +  B^o;  ,  ic  -  momentum  wheel  speed  -  ^  j - 

A 

T2  =  A2+V'  •  ,19) 

The  coefficients  A^  A^  are  determined  in  a  one  g  environment  and  to  relate  them  to  a 
zero  g  field  requires  a  reduction  in  their  value  to  compensate  for  the  reduction  in  coulomb  fric¬ 
tion  in  space.  The  coefficients  Bp  B^  will  vary  slightly  with  wheel  temperature.  This  is  taken 
into  account  when  simulating  various  conditions  in  which  the  wheel  operates. 
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Fig.  1.  Mode  PI:  Constant  wheel  speed. 


Fig.  2.  Mode  P2:  Normal  pitch  control. 
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Since  the  momentum  wheel  always  operates  at  a  positive  bias  speed  the  value  of  IRNP 
will  be  limited  within  a  fixed  range  of  values  for  all  operating  modes. 

2.  Mode  PI 

In  mode  PI  (constant  wheel  speed)  shown  in  Fig.  1,  the  input  to  the  wheel  speed  system 
IRNP  is  fixed  at  a  constant  value.  In  this  mode  the  wheel  speed  is  held  at  its  initial  condition 
value  unless  commanded  to  a  different  speed. 

3.  Mode  P2 

In  mode  P2  (normal  pitch  control)  shown  in  Fig.  2,  the  input  IRNP  to  the  wheel  speed  system 
is  a  function  of  the  earth  sensor  output  ISP.  The  control  law  for  this  mode  uses  momentum  ex¬ 
change  to  control  the  pitch  pointing.  In  addition,  a  coarse  momentum  expulsion  control  law  is 
used  to  regulate  wheel  speed  within  fixed  upper  and  lower  bounds  to  prevent  saturation.  The 
mode  P2  control  equations  are  given  below. 

IRNP  =  ISP(XP1)+  IQNP(XP2 )  .  (20) 

This  relation  is  updated  at  intervals  of  N  seconds  when  the  value  of  ISP  is  updated.  The  con¬ 
stants  XP1,  XP2  are  parameters  whose  values  are  adjusted  to  yield  optimum  pitch  performance 
in  mode  P2. 


IQNP  A  Yj  ISP  .  NP  =  integer  N  =  4  sec  .  (21) 

(N)NP 

Relation  (21)  is  updated  at  intervals  of  (N)(NP)  seconds.  The  parameter  NP  is  also  adjusted  to 
optimize  pointing  performance  in  conjunction  with  XP1,  XP2.  The  value  of  IQNP  is  limited  to 
the  bounds 

QNPMIN  <  IQNP  <  QNPMAX  (22  ) 

to  keep  the  wheel  speed  from  saturating. 


4.  Mode  P3 

Mode  P3  is  termed  the  "sun  acquisition"  mode.  In  this  mode  shown  in  Fig.  3,  the  output 
from  a  wide  angle  sun  sensor  is  used  to  automatically  stop  large  spin  rates  about  the  pitch  axis 


Fig.  3.  Mode  P3:  Sun  acquisition  mode. 
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and  point  a  reference  axis  to  the  sun  within  one  degree.  This  mode  uses  the  sun  sensor  refer¬ 
ence  Sj  as  an  input  to  the  mode  P3  control  law  logic.  This  mode  is  a  momentum  exchange  law 
which  uses  the  reaction  wheel  for  its  torque  source.  The  quantized  output,  1SSP,  of  the  sun 
reference  sensor  is  given  by  the  relation 

1SSP  =  [-S^DELTS]  ,  (23) 

where  DELTS  =  least  significant  bit  of  quantized  sun  sensor  output. 

The  mode  P3  control  law  generates  a  proportional  plus  derivative  switching  function,  ISIG. 
When  ISIG  is  less  than  a  threshold  value,  IDEL,  the  momentum  wheel  is  run  at  constant  speed. 
When  ISIG  exceeds  IDEL,  the  wheel  speed  is  increased  or  decreased  based  on  the  sign  of  ISIG. 

where 

ISIG  =  [1SSP-  ISSPO]  +  ISSP 

XP3  =  constant  design  parameter  controlling  the  amount  of  damping 
in  the  system 

ISSPO  =  value  of  ISSP  at  the  previous  sampling  interval  .  (24) 

The  function  ISIG  is  updated  at  intervals  of  N  seconds  and  is  nulled  about  a  threshold  value 
IDEL  with  the  control  law 


If 

1  ISIG  | 

> 

IDEL  , 

IRNPT  =  (MMODEP)  sgn(ISIG) 

(25) 

If 

|  ISIG  | 

IDEL 

IRNPT  =  IPNP 

(26) 

where 

IDEL  =  quantized  threshold  value  of  mode  P3  pointing  accuracy 

MMODEP  =  large  constant  representing  an  overriding  input  to  the  wheel 
speed  control  loop  which  effectively  uncouples  the  wheel 
speed  feedback 

IRNPT  =  quantized  input  to  the  wheel  speed  control  system,  re¬ 
placing  the  quantity  IRNP  used  in  mode  P2. 


5.  Mode  P4 

Mode  P4  is  termed  the  pitch  axis  "back-up11  mode.  In  this  mode  shown  in  Fig.  4,  the  ternary 
control  law  is  similar  in  nature  to  that  of  mode  P3,  but  now  the  system  is  a  momentum  expulsion 


1 16  -  T  -  ?  <  0  5  ] 


PITCH 

THRUSTERS 


Fig.  4.  Mode  P4  Back-up  mode. 
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system  with  the  thrusters  providing  the  control  torque  source.  This  mode  provides  a  partial 
baek-up  to  the  mode  P2  control  law.  It  is  assumed  that  the  momentum  wheel  is  running  at  con¬ 
stant  speed  in  this  mode.  The  control  function  ISIGB  is  given  by  the  relation 

ISIGB  =  tlSP_  ISP01  +  ISP  ’  (27) 

where 

XP4  =  constant  design  parameter  controlling  the  damping  in  the 
system 

ISPO  =  value  of  ISP  at  the  previous  sampling  interval 

IMP  =  integer  number  of  sampling  intervals  between  update  times; 
i.e.,  relation  (27)  is  updated  at  intervals  of  N(INP)  seconds. 

The  funetion  ISIGB  is  nulled  about  a  threshold  value  IDELB,  with  the  control  law 


If 

ISIGB  >  IDELB 

ISEQU  =  5 

1 

If 

ISIGB  <- IDELB 

ISEQU  =  1 

1 

If 

|  ISIGB  |  <  IDELB 

ISEQU  1  =  0 

(28) 

where 


IDELB  =  quantized  threshold  value  of  mode  P4  pitch  axis  pointing 
aeeuraey 

ISEQUj  =  logic  input  to  thruster  control  matrix  for  torque  about 

pitch  axis  (0,  1,  5  commands  designate  zero,  plus  and  minus 
torque,  respectively). 


6.  Mode  P5 

This  mode  is  called  the  ’’gyro"  control  mode.  It  is  a  tentative  mode  and  has  not  been  simu¬ 
lated  in  detail  yet.  A  preliminary  coarse  simulation  of  the  gyro  mode  as  envisioned  for  use  in 
LES-8/9  is  included  here.  The  rate  integrating  gyro  output  ISAMP  is  assumed  proportional  to 
the  integral  of  pitch  axis  rate,  i.e., 

ISAMP(IS)  =  5ELTAH  L  dt  '  (29) 

O 

To  be  compatible  with  the  earth  sensor  interface  the  gyro  output  is  quantized  with  the  same 
resolution  and  same  sampling  time  N  as  the  earth  sensors.  This  mode  simply  replaces  the 
earth  sensor  with  a  rate  gyro  output.  Detailed  simulation  of  the  gyro  dynamics  may  be  included 
at  a  later  time  if  necessary. 

B.  Pitch  Axis  Coarse  Momentum  Control 

In  addition  to  the  above  modes,  there  is  a  coarse  momentum  expulsion  limit  eontrol  used 
when  the  system  is  in  modes  P2  or  P3.  This  control  is  used  to  maintain  the  angular  momentum 
stored  in  the  wheel  within  ±10  percent  of  its  nominal  bias  value.  The  coarse  control  law  is  a 
binary  "on-off"  hysteresis  switch 
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Fig.  5.  IVIode  HI:  Damping  mode. 


where 


K  goin  of  D/A  converter  (-7-7 — ) 
d  bit 

K  goin  of  gimbol  torque  motor  ( -  ) 

t  A 

A 

=  goin  of  gimbol  torque  ompl  fier  (  "  i 


Fig.  6.  Mode  R2:  Normal  roll  control. 
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5  min 


ISEQU1  =  0 


ISEQU1  =  5  until  (Y5  -  Y^Q)  ^  0 


If  Y^  <  Y^  min 


ISEQU .  =  1  until  (Y_  -  Y.n)  >  0 
1  5  50 


(30) 


where 


ISEQU^  =  thruster  logic  input  for  pitch  torque  command 


Y^q  =  nominal  bias  momentum  value 


Y, 


5  min 


=  minimum  bias  momentum  allowable  before  dumping 


Y 


5  max 


=  maximum  bias  momentum  allowable  before  dumping. 


C.  Roll  Axis  Control  Modes 

There  are  4  operating  modes  available  for  controlling  the  satellite  attitude  about  the  roll 
axis.  Three  modes  utilize  momentum  exchange  capability  of  the  gimbal  control  system  and  one 
mode  uses  the  pulsed  plasma  thruster  for  direct  momentum  expulsion  attitude  control.  The 
control  laws  utilized  in  these  4  modes  are  listed  below. 


1  Mode  R1 


This  mode,  shown  in  Fig.  5,  is  termed  the  "damping  mode”  and  consists  primarily  of  a 
single  analog  rate  loop  used  to  generate  heavy  damping  of  gimbal  angle  rates.  In  Refs.  2,  4  it 
was  shown  that  the  existence  of  this  loop  caused  active  nutation  damping  at  all  times  and  incr- 
tially  stabilized  the  large  angular  momentum  vector  stored  in  the  momentum  wheel.  In  this 
mode  the  gimbal  damping  coefficient  XD^  and  spring  constant  XI)^  are  included  in  Eq.  (1)  as  fixed 
control  laws  and  the  gimbal  input  IRNR  =  0. 

2.  Mode  R2 

This  mode,  shown  in  Fig.  6,  is  termed  the  "normal”  roll  control  mode.  The  input  IRNR  to 
the  gimbal  control  system  is  a  function  of  the  earth  sensor  output  1SR.  The  control  law  for 
mode  R2  uses  momentum  exchange  between  the  gimbal  control  system  and  satellite  body  to  con¬ 
trol  the  roll  axis  pointing.  The  gimbal  torque  motor  provides  the  necessary  reaction  control 
torque  to  move  the  satellite  body  about  the  momentum  wheel  gimbal  axis  (roll  axis).  In  addition, 
a  coarse  momentum  expulsion  limit  control  law  is  used  to  regulate  maximum  gimbal  angle  mag¬ 
nitude  within  fixed  upper  and  lower  bounds  to  prevent  gimbal  saturation  and  excessive  yaw  axis 
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attitude  errors.  The  mode  R2  control  equations  are  given  below. 


IRNR  =  [ISR  +  (1QNR)  XR2]  (XR1 ) 


(31) 


This  relation  is  updated  at  intervals  of  N  seconds  when  the  value  of  ISR  is  updated.  The  con¬ 


stants  XRl  XR2  are  parameters  whose  values  are  adjusted  to  yield  optimum  roll  pointing  per¬ 
formance  in  mode  R2. 


IQNR  4  y  ISK  ,  NR  =  integer 
N(NR) 


N  =  4  sec 


(32) 
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Relation  (32)  is  updated  at  intervals  of  N(NR)  seconds.  The  parameter  NR  is  adjusted  to  opti¬ 
mize  pointing  performance  in  cooperation  with  XR1,  XR2.  The  values  of  IQNR  and  IRNll  are 
limited  to  the  bounds 

QNRMIN  <  IQNR  <  QNRMAX 

1 1RNR |  ^  RMAX  ,  (33) 

to  keep  the  gimbal  control  motor  from  saturating  and  thus  provide  no  damping. 

3.  Mode  R3 

Mode  R3,  shown  in  Fig.  7,  is  termed  the  Mgimbal  control"  mode.  In  this  mode  the  control 
laws  are  identical  to  those  used  in  mode  R2  except  that  the  earth  sensor  output  ISR  is  replaced 
by  the  quantized  gimbal  angle  sensor  output  Y^. 

ISR  =  -  Y4/DELTAR  .  (  34 ) 

This  mode  can  be  used  to  independently  control  the  satellite  orientation  about  the  roll  axis  as, 
for  instance,  in  an  acquisition  search  scan,  etc.  This  mode  is  a  momentum  exchange  system 
also  and  relies  upon  gimbal  control  motor  reaction  torque. 


Fig.  7.  Mode  R3:  Gimbal  control. 


4.  Mode  R4 

This  mode  is  termed  the  roll  axis  "back-up”  mode  (see  Fig.  8).  The  control  system  con¬ 
sists  of  a  binary  "on-off"  hysteresis  control  function  whose  sign  controls  the  satellite  attitude 
control  thrusters.  This  system  is  a  momentum  expulsion  system  whose  primary  function  is  to 
provide  a  back-up  capability  to  mode  R2.  This  mode  depends  upon  the  momentum  storage 


Fig.  8.  Mode  R4:  Back-up  mode. 


10 


capability  of  the  wheel  due  to  the  steady  state  precession  equations  relating  roll  axis  motion  with 
torque  about  the  yaw  axis.  The  control  law  for  this  mode  uses  the  roll  axis  earth  sensor  input 
S9  to  activate  the  yaw  thrusters. 

"  lS2l<Y2max  '  ISEQUj  =  0 

If  S9  >  Y9  ,  ISEQU-  =  3  until  S9  ^  0 

c.  d.  max  j  l. 

lf  S2<-Y2max  •  ISEQU3  =  7  until  S2  >  0  .  (35) 


where 


ISEQU^  =  thruster  logic  input  for  yaw  torque  command 


Y9  r  =  quantized  threshold  value  of  mode  R4  roll  axis  pointing 
accuracy. 

I).  Roll  Axis  Coarse  Momentum  Control 


In  addition  to  the  above  roll  control  modes,  there  is  a  coarse  momentum  expulsion  limit 
control  used  when  the  system  is  in  mode  R2.  This  control  is  used  to  maintain  the  satellite  an¬ 
gular  momentum  vector  orientation  perpendicular  to  the  orbit  plane  within  a  given  threshold 
value.  The  input  to  this  control  law  is  the  gimbal  angle  Y^.  This  control  law  maintains  |Y^| 
within  a  threshold  region  at  all  times  so  that  yaw  attitude  error  is  held  to  an  acceptable  value 
over  the  entire  orbit.  Sec  Ref.  4  for  a  description  of  how  Y^  couples  into  Y^  when  the  satellite 
is  in  mode  R2.  The  control  law  for  coarse  momentum  control  is  a  binary  "on-off”  hysteresis 
switch  as  follows 


If 

lY  1  ^  Y 

1  1  4  1  4  max 

ISEQU3  =  0 

(36a) 

lf 

Y  A  >  Ya 

4  4  max 

ISEQU3  =  3  until  Y.  $  0 

(36b) 

If 

Ya  <  -  y„ 

4  4  max 

ISEQU3  =  7  until  Y4  >  0 

(36c) 

where 


Y^  =  angular  threshold  value  of  mode  R2  yaw  axis  pointing 
accuracy. 


Note  that  relations  (36b)  or  (36c)  hold  until  the  value  of  Y^  reaches  zero  or  changes  sign  and 
then  control  is  shifted  to  relation  (36a). 


IV.  DESCRIPTION  OF  SIMULATION 

In  this  section,  the  subroutines  which  are  used  to  perform  the  simulation  are  briefly  described 
and  flow  diagrams  of  the  new  subroutines  are  given.  The  overall  simulation  is  performed  bv  1  3 
subroutines.  These  are  broken  into  three  basic  groups  according  to  their  functions. 

(a)  Subroutine  linkage  and  integration  subroutines 

Main  Program 
RK  (Runge-Kutta)1 
A  M  ( Ad  am  s-  M  ou  It  on ) 1 
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GENERATE 
PLOT  5 


RESET 

COUNTERS 
GO  TO  10 

nr 


Fig.  9.  Flow  diagram  of  main  program. 
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(b)  Satellite  dynamics,  kinematics,  sensors  and  control  law  subroutines 

DER1V 

DIGIT 

TORK 

TORQUE 

UNLOAD 

(c)  Output  format  and  plotting  subroutines 

OR  1 
OR  2 
OUTPUT 
FRAME  V 
PRINT  V 

A.  Mam  Program  of  the  Simulation 

The  main  routine  sets  up  the  initial  conditions,  reads  each  data  set  in  turn,  calls  the  simula¬ 
tion  routines,  and  plots  the  results  (see  Fig.  9).  There  are  several  routines  called  in  various 
sections  of  the  program  which  are  in  the  Lincoln  Library.  They  are  DUMPV,  TIMHR,^  REREAD, 
STOIDV,  FRAMEV,  PRINT V,  PLTND. 

The  input  is  read  in  via  NAMELIST/PETE/.  All  variables  are  computed  in  standard  engi¬ 
neering  units,  degrees,  degrees/second,  rpm  for  the  wheel  speed.  Those  variables  in  the  input 
list  whose  units  have  to  be  altered  for  the  simulations  are  stored  as  they  are  initialized  and  new 
variables  hold  the  converted  values.  The  variable  names  differ  by  0  (zero)  as  the  final  character 
of  the  variable  name.  Following  the  namelist,  two  title  cards  are  read  in.  The  program  stores 
all  80  columns  of  the  first  card  and  the  first  48  columns  of  the  second  card.  Successive  data  sets 
may  be  added  for  each  simulation  desired.  The  program  may  be  terminated  in  either  of  two  ways: 
(1 )  the  absence  of  data  to  be  read  in  terminates  the  program  normally,  (2)  the  namelist  may  be 
read  in  with  the  logical  variable  FIN  set  to  TRUE  :  FIN  =  T. 

The  program  was  originally  written  to  produce  punched  output  with  the  intention  of  continuing 
a  particular  simulation.  Those  write  statements  in  subroutine  AM  on  logical  unit  7  are  currently 
commented  out.  To  read  these  cards  back  in,  the  user  should  initial  all  variables  in  the  name- 
1  i st  as  they  originally  were  with  the  exception  of  the  logical  variable  TOO  :  TOO  =  T.  The  punched 
cards  follow  the  namelist  and  precede  the  titles  in  the  input  stream.  When  this  method  of  initial¬ 
ization  is  used,  the  array  VO  has  units  used  by  the  program  rather  than  engineering  units.  For 
a  successive  data  set,  all  of  Y0  should  be  initialized. 

To  perform  the  requested  simulation  the  program  makes  an  initial  call  to  TORK  to  initialize 
constants  for  the  thrusting  sequencing  routine  TORQUE  and  UNLOAD.  These  constants  are  used 
via  the  calls  to  the  entry  point  THRUST  in  TORK.  An  initial  call  to  OUTPUT  stores  the  initial 
values  of  V  to  be  plotted.  The  sequence  of  calls  to  RK  and  AM  perform  the  simulation.  Control 
returns  to  MAIN  when  T  exceeds  TBOUND.  The  stored  arrays  PY1,  PY2 .  .  .  .  PYn  are  plotted 
via  calls  to  GR1.  NFR  controls  the  number  of  frames  the  data  are  to  be  plotted  on;  PT  is  the 
array  of  times,  PYi  is  the  corresponding  array  of  data.  If  PYi  is  constant  throughout,  no  plot 
can  be  generated.  I  ERR  is  returned  nonzero.  The  calls  to  PR1NTV  label  the  last  frame  of  eacli 
set  of  frames  per  array  plotted.  The  first  and  last  frame  for  each  simulation  are  the  input 
parameters  and  titles. 
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HS7-24.11 


Fig.  10.  Main  flow  of  subroutine  digit. 
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After  resetting  the  appropriate  parameters  and  counters,  program  control  returns  to  the 
reader  for  another  set  of  data  for  the  next  simulation.  An  empty  reader  terminates  the  program. 

The  current  size  of  the  program  requires  that  simulations  totaling  2000  seconds  or  less  be 
run  as  Class  C  jobs  under  Lincoln's  MVT,  longer  simulations  are  Class  F. 

B.  Major  Subroutines 

1.  DERIV  Subroutine 

Subroutine  DERIV  is  called  by  RK  and  AM  once  for  each  step  in  the  integration.  The  call¬ 
ing  sequence  is  Y,  TOOT.  Y  is  the  array  of  values  coming  in;  YDOT  is  the  array  of  values  re¬ 
turned.  In  addition  to  calculating  TOOT  for  each  defined  entry  of  the  array,  DERIV  sets  up  the 
parameters  needed  by  DIGIT,  calls  DIGIT,  and  updates  the  motor  torque  T^  using  the  results  of 
DIGIT  every  Tp  seconds. 

The  parameters  IB  and  IBE  are  flags  for  the  initial  step  of  each  simulation.  They  are  zero 
until  used  once,  then  they  are  nonzero  until  a  new  simulation  is  begun. 

The  state  variables  Y,  TOOT  are  dimensioned  20  in  the  calling  programs.  Currently,  the 
program  uses  1  2  of  these  20  locations.  To  add  to  the  existing  system  of  differential  equations, 
one  need  define  YDOT(i)  in  DERIV,  where  12  <  i  ^  20.  The  variable  NEQ  is  initialized  on 
BLOCK  DATA  and  can  be  changed  in  the  main  program  via  NAMELIST.  The  parameter  NEQ  is 
the  number  of  equations  in  the  system  being  solved. 

2.  DIGIT  Subroutine 

Subroutine  DIGIT  contains  the  mathematical  model  of  the  actual  digital  control  system  on  the 
pitch  and  roll  axes.  It  receives  from  DERIV  the  current  values  of  S(l),  S(2),  Y(4),  Y(5),  and 
Y(10).  It  returns  to  DERIV  the  values  of  the  sensors  for  the  pitch  and  roll  axes,  IRNP  and  IRNR. 

The  main  flow  of  this  routine  is  shown  in  Fig.  10  and  is  constructed  with  three  major  blocks. 
The  first  block  is  executed  once  at  the  beginning  of  each  simulation.  In  this  block  all  parameters 
necessary  for  the  digital  model  and  dependent  on  the  particular  input  data  set  are  calculated  and 
all  counters  are  initialized. 

Parameter  IB  flags  whether  or  not  the  first  block  has  been  executed.  The  second  block 
stores  the  sensor  output  every  quarter  second  [see  Figs.  11(a)  and  11(b)).  The  third  block  aver¬ 
ages  the  sensor  output  over  4  second  intervals  and  uses  these  averages  to  calculate  the  returned 
values  for  the  controlling  system  of  equations. 

In  the  first  block,  SIGMA  is  converted  from  degrees  to  least  significant  bits,  and  its  value 
is  adjusted  for  4  second  averaging  effects  by  being  multiplied  by  4.  Maximum  and  minimum 
saturation  values  are  calculated  for  the  integrator  registers  and  these  values  are  returned  to 
DERIV.  The  variable  T  is  the  current  time  as  calculated  in  RK  and  AM.  TZERO  is  the  initial 
time  of  each  quarter  second  sampling  period.  IS  is  the  incremented  index  to  store  the  sampled 
information.  IS  is  set  to  zero  initially  and  after  each  set  of  16  sensor  samples. 

If  MODEP  is  3,  there  is  a  delay  in  returning  the  pitch  sensor  output  back  to  DERIV.  The  de¬ 
lay  is  controlled  by  TIMDEL,  an  input  parameter.  IP3  delays  the  program  logic  flow  until  the 
sensor  output  is  generated  before  checking  to  see  if  elapsed  time  equals  or  exceeds  the  time  de¬ 
lay.  TP3  is  the  time  that  the  sensor  output  was  generated. 

The  second  block  of  DIGIT  stores  the  sensor  output  every  quarter  second  to  be  eventually 
averaged  (see  Figs.  11(c)  and  11(d).  These  are  two  principal  controlling  statements.  The  first 
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Fig.  11(a).  Pitch  axis  sensor  flow  in  digit. 
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Fig.  11(b).  Roll  axis  sensor  flow  in  digit. 
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Fig.  11(c). 


Roll  axis  logic  flow  in  digit. 
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Fig.  11(d).  Pitch  axis  logic  flow  in  digit. 
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is  a  computer  GO  TO  for  the  roll  axis.  The  second  is  a  computer  GO  TO  for  the  pitch  axis,  i.e., 
GO  TO  (19,  29,  19),  MODE R. 

When  MODER  -  1  or  3,  the  program  logic  flow  goes  to  labeled  statement  19.  Likewise,  when 
MODER  2,  program  control  branches  to  labeled  statement  29. 


Value 

Meaning 

MODER 

1 

DAMPING  MODE 

2 

POINTING  MODE 

3 

GIMBAL  CONTROL  MODE 

MODEP 

1 

CONSTANT  SPEED 

2 

POINTING  MODE 

3 

ACQUISITION  MODE 

4 

PITCH  BACK-UP  MODE 

5 

RATE  INTEGRATING  GYRO  MODE 

To  increase  the  number  of  modes  for  either  roll  or  pitch  axis,  another  labeled  control  sec¬ 
tion  must  be  added  and  the  label  put  in  the  list  for  the  appropriate  computed  GO  TO  statement. 

In  DERIV,  regardless  of  which  roll  axis  is  used,  control  passes  eventually  to  labeled  statement 
19,  the  computed  GO  TO  for  the  pitch  axis.  The  various  pitch  modes  finally  pass  control  to 
labeled  statement  18  where  TZERO  is  set,  and  IS  is  checked  to  see  if  16  samples  have  been 
stored.  If  less  than  16  samples  are  in  storage,  control  returns  to  DERIV.  Otherwise,  control 
passes  to  the  third  block  in  DIGIT. 

For  the  roll  axis,  no  sampling  takes  place  in  modes  1  and  3.  For  mode  2,  the  values  ot 
Y(2),  (S(2)  in  DERIV)  are  stored.  The  decimal  fraction  of  Y(2)  is  truncated;  the  integrals  re¬ 
maining  are  then  converted  from  radians  to  least  significant  bits.  For  a  nonzero  SIGMA,  a 
random  number  from  a  Gaussian  distribution  is  added  to  Y(2).  The  storage  array  is  ISAMR. 
According  to  the  value  of  Y(l),  (S(l)  in  DERIV),  limits  of  the  field  of  view  for  the  sensor  are  de¬ 
termined.  If  Y(l)  is  outside  the  field  of  view,  FOV,  a  zero  is  stored.  If  the  absolute  value  of 
ISAMR  lies  between  1024  and  2047,  the  difference  between  2048  and  ISAMR  is  stored  with  the 
signs  of  the  original  value.  This  process  is  referred  to  as  the  "fold  over"  sensor  characteristic. 
When  the  roll  axis  sensor  output  is  stored,  the  program  then  repeats  the  procedure  for  the  pitch 
axis  sensor. 

For  the  pitch  axis,  no  sampling  takes  place  for  modes  1  and  3.  For  mode  9,  the  variable 
Y(10)  (the  rate  gyro  output)  is  converted  from  radians  to  least  significant  bits  and  stored  in 
ISAMP.  For  pitch  modes  2  and  4  the  current  value  of  Y(  1)  plus  any  random  noise  is  stored  in 
ISAMP.  The  random  noise  is  selected  from  a  Gaussian  distribution  with  standard  deviation, 
SIGMA.  For  SIGMA  equal  to  zero  no  noise  is  added.  The  program  compares  SIGMA  ±  EPS 
(where  EPS  is  a  small  number)  with  zero,  since  testing  on  equality  with  real  numbers  is  not  al¬ 
ways  reliable.  If  Y(2)  indicates  that  the  sensor  is  out  of  the  field  of  view,  ISAMP  is  set  to  zero. 
The  fold  over  sensor  characteristic  is  identical  for  both  the  pitch  and  roll  axis  sensors. 

After  the  pitch  axis  information  is  stored,  TZERO  is  reset.  IS  is  compared  to  see  if  16  sam¬ 
ples  have  been  taken.  The  16  samples  parallel  the  actual  16  samples  taken  every  1/4  second  for 
4  seconds.  A  counter  rather  than  absolute  time  is  used  because  of  the  variety  of  time  increments 
possible  to  be  used  by  AM.  Four  seconds  could  mean  15  or  17  samples  taken.  If  IS  is  less  than 
16,  program  control  returns  to  DIGIT. 
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When  IS  equals  16,  program  control  passes  to  the  third  block  of  DIGIT.  The  integer  N  is  a 

continuous  counter  of  the  number  of  times  sensor  averaging  has  taken  place.  Thus,  certain 

th 

calculations  can  be  done  every  1  change  in  N  by  a  comparison  of  N  mod  i  with  zero. 

In  the  third  block  of  DIGIT,  the  two  principal  control  statements  are  computed  GO  TO's  for 
roll  and  pitch  axis.  Any  additional  modes  added  in  the  second  block  must  also  be  added  to  these 
GO  TO's  even  though  the  averaging  logic  is  the  same  as  some  previous  mode.  Whatever  roll 
axis  mode  is  used,  control  eventually  passes  to  labeled  statement  69.  Statement  69  is  the  com¬ 
puted  GO  TO  for  the  pitch  axis.  Each  pitch  mode  eventually  returns  to  labeled  statement  68. 

Here  the  counter  IS  is  reset  to  zero  and  control  returns  to  DERIV. 

For  the  roll  axis,  mode  1  returns  a  zero  in  the  output  IRNR.  For  mode  2,  the  16  stored 

sensor  values  are  averaged  as  ISR.  For  mode  3,  M4)  is  converted  from  radians  to  degrees  and 
then  quantized  and  stored  as  ISR.  The  variables  ISR  and  IQNR  are  used  to  calculate  IRNR.  IQNR 

1L 

is  updated  by  ISR  every  NRin  time  and  is  limited  by  QNRMAX  and  QNRMIN.  The  absolute  value 
of  IRNR  is  limited  by  RMAX  and  is  returned  to  DIGIT.  After  this  operation,  control  passes  to 
the  computed  GO  TO  for  the  pitch  axis. 

For  the  pitch  axis,  mode  1  returns  the  initial  wheel  speed  with  appropriate  unit  manipulations 

via  IINT  as  IRNP.  For  mode  2,  the  16  samples  in  ISAMP  are  averaged  and  the  result  stored  as 

ISP.  INDEX  is  a  counter  for  storage  of  time  and  sensor  output  to  be  plotted.  IRNP  is  calculated 

th 

using  ISP  and  IQNP  where  IQNP  is  updated  by  ISP  every  NP  time  through  the  loop.  IQNP  is 
bounded  by  QNPMAX  and  QNPMIN.  For  mode  5,  the  output  of  sensor  one,  Y(l)  is  bounded  by 
±  and  converted  from  radians  to  least  significant  bits  and  stored  as  ISSP.  For  the  time  delay 
option  TP3  is  set  to  T,  IP3  is  set  to  1. 

For  mode  4,  control  law  calculations  take  place  every  INP^  time  and  this  affects  the  values 
of  ISEQU.  ISEQU  controls  the  sequence  of  the  firing  of  the  thrusters.  In  this  mode  IRNP  is 
returned  as  in  mode  1. 

3.  TORK,  TORQUE,  and  UNLOAD  Subroutines 

The  three  subroutines  TORK,  TORQUE,  and  UNLOAD  provide  DERIV  with  the  appropriate 
torques,  LI,  L2,  L3,  which  simulate  the  firing  of  the  thrusters.  Subroutine  TORK  is  called 
once  per  simulation  from  the  main  program  to  establish  certain  tables  dependent  on  the  physical 
location  of  the  thrusters  on  the  satellite  body.  Subroutine  TORQUE  is  called  once  per  integra¬ 
tion  step  by  both  AM  and  RK.  UNLOAD  is  called  each  time  TORQUE  is  entered.  If  it  is  time 
to  fire  a  thruster,  TORQUE  calls  TORK  via  ENTRY  THRUST.  At  this  time  torques  LI,  L2,  L.3 
are  updated  for  DERIV. 

The  thruster  torques  are  passed  through  COM  MON  /CONTROL/  and  they  have  different 
names  in  each  routine. 


Subroutine 

Variable 

Name 

TORK 

T(l) 

T(2) 

T(  3) 

TORQUE 

L(  1) 

L(2) 

L(3) 

DERIV 

LI 

L2 

L3 

In  each  routine  they  are  REAL *8  variables. 

Subroutine  TORK  accepts  as  input  the  distance  in  inches  of  the  thrusters  from  the  origin  of 
the  satellite  reference  frame.  TORK  establishes  a  torque  look-up  table,  TABLE,  first  by  filling 
in  the  signs  (+  or  -)  of  the  torque  components  for  each  thruster  in  a  12  x  3  array.  It  then 
computes  the  magnitudes  of  the  torques  and  replaces  each  entry  with  the  torque  components 
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along  the  Z^,  Z^,  and  Z^  axes.  THETA  is  the  angle  of  thrust  with  respect  to  Z^Z^  plane  and  PHI 
is  the  angle  of  thrust  with  respect  to  Z^Z^  plane.  FORCE  is  the  average  force  per  firing  pulse 
for  each  thruster.  The  ENTRY  THRUST  routine  adds  the  component  torques  of  each  of  the 
thrusters  fired  to  the  total  components  stored  in  COMMON/CONTROL/. 

Subroutine  TORQUE  contains  the  logic  to  keep  track  of  which  thruster  is  being  fired,  along 
with  the  length  of  firing  time,  and  determines  the  sequence  of  the  firing.  Two  arrays  initialized 
in  BLOCK  DATA  arc  critical  to  the  functioning  of  TORQUE. 

ASEQ  is  an  integer  array  of  numbers  from  0  to  8  which  specifies  the  sequence  of  torquing 
activity.  The  program  will  specify  thruster  torque  about  each  of  the  three  satellite  axes  for 
100  seconds.  At  the  end  of  each  100  second  interval,  the  program  will  automatically  shift  to  the 
next  axis  indicated.  The  following  table  establishes  a  correspondence  between  ASEQ  array 
values  and  common  torque  about  the  axes. 


Torque 

no  torque 
+21  torque 
+22  torque 
+23  torque 
+  22  stationkeeping 
-21  torque 

—  22  torque 

—  23  torque 

—  22  stationkeeping 


ASEQ  Index 

0 

1 

2 

3 

4 

5 

6 

7 

8 


The  thruster  selection  rules  can  be  easily  changed.  The  thruster  selection  rules  are  imple¬ 
mented  by  a  thruster  selection  table  initialized  by  BLOCK  DATA.  The  thruster  selection  table 
is  kept  in  the  labeled  common  block,  SELECT.  SELECT  is  an  8  x  4  integer  array.  The  first 
index  specifies  the  desired  action  according  to  the  previous  tabic  while  the  second  index  specifies 
the  selected  thrusters.  The  thrusters  and  the  indices  used  to  specify  them  are  listed  in  the  fol¬ 
lowing  table  for  reference. 


Thruster 


Select  Index 


A, 


C. 


1 

2 

3 

4 

5 

6 

7 

8 
9 

10 

11 

12 
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This  table  is  used  for  the  selection  of  up  to  four  thrusters  to  provide  torque  about  each 

axis. 

ASEQU  is  passed  in  common/SEQU/  and  SELECT  in  common/TABLES/.  ATIME  and  A  LIMIT 
are  the  current  length  of  time  of  firing  and  maximum  allowable  time  of  firing  about  the  current 
axis.  TTIME  and  TLIMIT  are  the  current  length  of  time  of  firing  and  maximum  allowable  time 
of  firing  with  the  current  thruster.  APOINT  specifies  the  current  axis.  TCUR  specifies  the  cur¬ 
rent  thruster.  The  call  to  THRUST  is  made  with  the  appropriate  value  of  IFIRE  taken  from  an 
appropriate  place  in  SELECT.  With  the  three  forces  updated,  control  returns  to  DERIV. 

At  the  beginning  of  TORQUE  is  a  call  to  subroutine  UNLOAD  (see  Fig.  12).  The  values  of 
ASEQU  are  updated  depending  on  the  values  of  MODER  and  MODEP  (ASEQU  is  called  ISEQU  in 
UNLOAD).  First  the  roll  axis  modes  are  examined,  then  the  pitch  axis  modes.  If  the  roll  axis 
mode  is  1  or  3,  there  is  a  choice  of  a  back-up  mode,  MODE  -  2.  If  there  is  no  back-up  on 
these  modes,  UNLOAD  does  nothing.  If  MODE  -  2,  ISEQU(3)  is  changed  to  0  or  3  or  7  depending 
on  the  previous  value  of  ISEQIK3),  the  current  value  of  Y(4)  and  the  maximum  limit  on  Y(4)(YMAX). 
In  the  back-up  mode  for  the  roll  axis,  ISEQU(3)  becomes  0,  3,  or  7  depending  on  the  previous 
value  of  ISEQU (3),  the  current  value  of  Y(2)  and  the  limiting  Y2MAX.  This  is  referred  to  as  the 
gimbal  dump  logic. 

For  the  pitch  axis  in  modes  1  and  2  the  value  of  ISEQU(l)  is  changed  to  0,  1,  or  5  depending 
on  the  previous  value  of  ISEQU(l),  the  current  value  of  Y(5),  the  upper  and  lower  limits,  Y5MAX 
and  Y5MIN,  and  the  initial  wheel  speed  Y050.  This  is  referred  to  as  the  wheel  dump  logic. 

C.  Output  Format  and  Plotting  Subroutines 

The  printed  and  optional  punched  output  is  produced  in  subroutine  AM.  The  plotted  output  is 
stored  by  OUTPUT  and  plotted  by  GR1  and  GR2.  OUTPUT  is  called  periodically  by  AM,  every 
2  seconds.  To  alter  the  frequency,  MPT  should  be  changed  in  BLOCKDATA  and  AM.  The  fre¬ 
quency  of  points  plotted  is  related  to  the  initial  step  size  HO. 

r  256  *  HO 

frequency  =  MpT — 

Printed  output  is  produced  every  TBOUND-TO/lO  seconds,  to  yield  10  sets  total  per  simulation. 
Punched  output  is  produced  at  the  end  of  each  simulation.  The  punched  output  is  in  Z  format  as 
it  is  intended  for  machine  use  only.  The  punched  output  does  not  include  the  current  sensor  in¬ 
formation  from  DIGIT. 

Subroutine  GR1  is  called  from  the  main  program  once  for  each  variable  to  be  plotted.  The 
calling  sequence  is  NFR,  X,  Y.  NFR  is  the  number  of  frames  to  spread  the  data  over;  X  is  the 
array  of  horizontal  values;  Y  is  the  array  of  vertical  values.  GR1  calls  GR2  passing  necessary 
parameters  via  COMMON/GR/.  GR2  calls  the  SC4020  routines  necessary  to  produce  a  grid, 
points,  and  lines  connecting  the  points.  If  all  the  Y  values  are  constant,  no  grid  can  be  drawn, 
as  the  scale  on  each  grid  is  dependent  on  the  range  of  the  Y  values.  In  case  no  grid  can  be 
drawn,  a  flag,  IERR,  is  returned  non-negative.  No  further  processing  is  performed  in  GR2  and 
the  flag  is  returned  to  the  main  program  via  common/SKIP/  so  that  the  printing  of  the  titles  is 
skipped  also. 

If  it  is  desired  to  add  the  plotting  of  another  variable,  the  variable  should  be  stored  in  an 
array  in  subroutine  OUTPUT.  The  array  should  be  dimensioned  5001,  should  be  REAL  *  4, 
and  should  be  in  common  /OTPT/  in  both  OUTPUT  and  in  MAIN.  In  MAIN  two  additional  cards 
are  needed,  one  call  to  GR1  and  one  conditional  call  to  PRINTV  to  produce  a  title  on  the  plot. 
Deleting  a  frame  of  output  is  similar. 
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return"^ 


Fig.  12.  Flow  diagram  of  subroutine  unload. 
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V.  USE  OF  THE  PROGRAM 


The  required  program  input  variables  along  with  typical  values  used  and  the  output  data 
format  are  described  in  this  section.  For  each  computer  run  input  data  are  printed  out  at  the 
beginning  of  the  run  to  identify  the  parameter  values  used.  The  input  variables  are  listed  below 
in  the  same  order  as  required  in  the  program. 


A.  Input  Variables 

HO  3=  initial  computation  step  size  (seconds) 

TBOUND  -  final  computation  time  (seconds) 

TO  -  initial  computation  time  (seconds) 

VO  -  initial  conditions  of  variables  Y1  through  Yl2,  respectively, 
at  t  -  TO 

NFR  -  integer  number  of  frames  of  plotted  output  per  simulation 

FIN  -  F  or  T,  logical  false  or  true  which  stops  program 

TOO  -  F  or  T,  logical  false  or  true  which  is  used  to  signify  continua¬ 
tion  of  a  run 

GLB  -  greatest  lower  bound  limit  on  integration  error  over  one 
step  ( 10“7) 

LUB  -  least  upper  bound  limit  on  integration  error  for  one  step  (10~^) 

XJ  principal  inertias  of  satellite  and  reaction  wheel  rotor  (XJ  1, 

XJ  2,  XJ  3,  XJ4) 


Z  2 


Z1 


ASEQl 

XP1 
XP2 
XD1 
X  D  2 
XD3 
X  R  1 
XR2 
MODE 


SIGMA 

NR 

MODER 


thruster  location  distance  from  satellite  center  of  gravity  on 
coordinate  Z2  (inches),  one  dimension 

thruster  location  distances  from  satellite  center  of  gravity  on 
coordinate  Z1  (inches),  two  dimensions 

three  integers  representing  the  initial  values  of  thruster  firing 
command  about  pitch,  roll,  yaw  axes,  respectively 

pitch  axis  proportional  gain  parameter  (integer) 

pitch  axis  integral  gain  parameter 

nutation  damping  coefficient  parameter 

gimbal  flexible  spring  constant 

not  being  used  (spare  parameter) 

roll  axis  proportional  gain  parameter 

roll  axis  integral  gain  parameter 

(1  or  2)  roll  axis  back-up  enable  switch  indicating  normal  or 
back-up  control  mode  operation 

RMS  value  of  random  noise  output  of  earth  sensor  (deg) 

roll  axis  integral  gain  sampling  ratio  (integer) 

integer  representing  desired  roll  axis  operating  mode  (1-4) 


25 


MODEP  integer  representing  desired  pitch  axis  operating  mode  (1-5) 

MMODEP  -  integer  number  input  to  wheel  speed  control  in  pitch  acquisition 
mode 

IDEE  integer  number  representing  deadband  in  pitch  acquisition  mode 
(bits) 

TIMDEL  =  time  delay  in  updating  pitch  acquisition  mode  logic  after  each 
sensor  sample  (seconds) 

NP  =  pitch  axis  integral  gain  sampling  ratio  (integer) 

M2  ~  gimbal  misalignment  angle  about  the  Z^  axis  (radians) 

M3  =  gimbal  misalignment  angle  about  the  Z^  axis  (radians) 

XP3  =  pitch  axis  derived  rate  gain  parameter  in  acquisition  mode 
(integer) 

LSB  =  wheel  speed  period  resolution  (bits/second) 

TD1  =  external  torque  about  pitch  axis  (ft-lbs) 

TD3  external  torque  about  yaw  axis  (ft-lbs) 

Y4MAX0  =  maximum  gimbal  angle  allowable  in  roll  axis  normal  mode  before 
momentum  dumping  (degrees) 

Y5MIN0  minimum  wheel  speed  allowable  before  momentum  dumping 
starts  (rpm) 

Y5MAX0  =  maximum  wheel  speed  allowable  before  momentum  dumping 
starts  (rpm) 

Y2MAX0  deadband  in  roll  axis  back-up  mode  (degrees) 

Y5N0  nominal  bias  wheel  speed  (rpm) 

XP4  pitch  axis  derived  rate  gain  parameter  in  back-up  mode  (integer) 

1DEL13  =  integer  number  representing  deadband  in  pitch  back-up  mode 
(bits) 

INP  =  sampling  ratio  of  pitch  back-up  logic  to  sensor  sampling  (integer) 

Typical  values  for  the  above  defined  variables  for  simulation  of  the  LES-8/9  current  configura¬ 
tion  are  given  below: 

MO  =  0.25  sec,  T BOUND  =  300  —  1200  sec,  TO  0.0 

Y0  (initial  conditions  for  all  state  variables),  Y^q  =  initial 
pitch  axis  error  (degrees) 

Y?n  initial  roll  axis  error  (degrees),  Y  n  -  initial  yaw  axis  error 
(degrees)  3 

Y4q  =  initial  gimbal  angle  (degrees),  Y^Q  -  initial  wheel  speed  (rpm) 

Y,n  =  initial  pitch  axis  body  rate  (deg/sec),  Y?  =  initial  roll  axis 
body  rate  (deg/sec) 

Yft0  initial  yaw  axis  body  rate  (deg/sec),  Y^Q  not  used  -  0 

Yioo  =  initial  value  of  integral  of  Y^,  =  initial  1TAE  value  for 

pitch  error 
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.  LS<tgl 


\  initial  ITAE  value  for  roll  error,  NFR  =  1,  FIN  -  F, 

TOO  =  T,  GLR  =  10-7,  LUB  =  10-5, 

X.l  =  (values  of  inertias):  XJ  1  =  120  slug-ft2,  XJ2  130  slug-ft2 


XJ  3  28  slug-ft2,  XJ 4  -  0.065  slug-ft2,  Z2  =  20  inches, 

Z1  14.75,  16.25  inches,  ASEQU  =  0,  0,  0 
XP1  2,  XP2  =  0.125,  XD1  6. 0  ft-lbs -sec/rad,  XD2  =  0.  3  ft-lbs/rad 
XD3  =  0,  XR1  =  0.25,  XR2  =  0.5  MODE  =  1,  SIGMA  =  0.03  degrees 
NR  =  2,  MODER  =  2,  MODEP  =  2,  MMODEP  =  200,000,  IDEL  -  24, 
TIM  DEL  0,  NP  =  3,  M2  =  0,  M3  =  0,  XP3  6, 

LSB  =  500,000,  TD1  0,  TD3  =  0,  Y4MAX0  =  0.1  degrees, 

Y5MIN0  =  1000  rpm,  Y5MAX0  -  1200  rpm,  Y2MAX0  =  0.1  deg, 

Y5N0  1100  rpm,  XP4  =  200,  IDELB  -  20,  INP  =  5. 


B.  Output  Data 

Output  data  from  the  program  described  here  is  in  the  form  of  plots.  The  subroutines  used 
to  generate  the  output  plots  are  described  in  Sec.  IV-C.  Typical  transient  response  plots  of  the 
variables  Y^,  Y^,  versus  times  are  illustrated  in  Figs.  13,  14,  15,  respectively.  The  input 

data  for  these  runs  are  listed  in  Sec.  V-A.  The  plot  coordinates  are  scaled  in  common  units  for 
ease  of  analysis;  deg,  deg/sec,  bits,  rpm,  seconds. 

In  addition  to  the  plots,  each  computer  run  contains  a  torque  table  printed  out  at  the  top  of 
the  input  variable  listing.  This  table  is  the  output  from  subroutine  TORK  and  consists  of  a 


Fig.  1  3.  Computer  plot  of  pitch  transient 
response. 


Fig.  14.  Computer  plot  of  roll  transient 
response. 
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Fig.  15.  Computer  plot  of  reaction  wheel 
speed  response  for  Fig.  13. 


3x12  matrix  of  torque  components.  This  matrix  consists  of  the  components  of  torque  impulse 
about  each  principal  axis  of  the  satellite  resulting  from  each  thruster  on  the  satellite. 

Also,  included  with  each  run  is  a  printout  of  all  variable  values  at  a  few  discrete  points  in 
the  solution  time.  These  data  are  strictly  for  diagnostic  purposes  in  monitoring  the  performance 
of  the  simulation.  At  the  end  of  the  output  data  is  printed  the  final  value  of  the  performance  in¬ 
dex  Yjj  and  Y^.  These  indexes  are  useful  for  fine  evaluation  of  control  system  transient  re¬ 
sponse  performance. 
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APPENDIX 


,HORT  TERM  0  I  MU1  A  T  I  CM  FOR  LLS-7  TEM00040 

IMPLICIT  K  E  *  L  *  8  (A-H,0-Z)  TEMQOObO 

D I  Ml  NS  I  ON  T  *  TLl (  lb)  , Y( 20) , YOC 20)  , ?( 4 ) , C( 4)  f FY( 20 , 5  )  , XJ( 4 ) f L ABEL ( 49TFM00060 
/!(?)  TFM00070 


2) 


or  ALH 


LUG  ICAL  F  I  4, TOO, Tf  RM 
I  MTt  GF  R  ASl  "LM  3  )  ,  APUl  NT 
K  E  A  L  *  R  M  2  ,  M  ^ 

PYl(*'001)fPY2(  soon  ,  PY  3  (  5001  )  ,  P  Y4  (  800  l  )  ,  P  Yb  (  SOOl  )  ,  PT  (  b()0  l  ) 
TlMFI,  T I  ME  2  *  LIT  I  ME,  I  TIME 
PY  7(  ‘'001  ) 

XT  ,YT 
LUB,  K 

/CUE  r /P,C , T BOUND, WR  , MPT ,NCT, NSTEP, 

/QTPF/PT ,PYI  ,PY2,PY3,PY4,PYb 
/PAR 


RE  AL  *4 
RE AL*4 
P.  C  A  L  *  H 
OCOMM.ON 

1 

2 
* 

4 

8 

COMMON 

COMMON 

common 

COMMON 


NEC 


AM/XJ,XJ23,XJ'3l  ,  XJ21  t  Tl  ,  T2t  OMtGA(),XPi  ,XP2,XDi,XD2,X03, 
XR1  ,  vj<2 


TT1MF,  ASEUJ,  AROINT 


/A  W'OM/HSCO 
/SEO1'/  H,  ATIML, 

/GR  /  OUM (  5 ) ,  NPL 
/  E  R  E  r  /  YCbO,  I  NDL  X 
/ PL  l) T  /  X  T  (8000  )  ,  Y  T  (  8000  ) 

/ AL  I  °N/  M2,  M  1 
/SKl*V  I  ERR 
COMMON /PI  rc»  /XP4  , I  DEL k , I NP 

COMMON  / E  L  L  r  N /  TIM DEL, TIM, SIGMA,  ID , NP, MODE R,NR,IRNP,IRNK,MCUFP, 
2  I  DEL  ,MMCOLP,  I P N P , X P  3 , L  S b 

COMMON 
COMMON 
COMMON 

REAL  *4  SP ( b°0  1  , 2 ) 

/RLTTER/  S ( 2 ) , SP 

TOC, I  P  /T, F/,Y()/20*0.U0/, GIB, LtB/UO-7, 1.0-8/ 

PI  /3.  l4l5N2t838b97'T3/,  Zi,  12  /14.7b,  *' 

4AMLL I S  T  /Pr  TE/hC,  THOUNl  ,  TO, YO 

12, L  1 


/  T  L  M  n  /  P  Y  7 
/DISTUB/  TDl«r03 

/l)UMn/Y4MAX  ,  YbMlN,  YbMAX,  Y2MAX,  Y5N,M00E 


COMMuN 
DATA 
GA  T  A 


CALL 
146, 0 

Call 

CALL 


16.2b,  20.0/ 

),NFR , F I N, TCO, GLB, LUB, XJ , 


S  TO  I UV  f  ’ATTITUDE  CONTROL  SYSTEM  PLTE  SMITH  D- 3 7 1  XSOIS’ 

I 

BUM  P V  '264) 

REREAD  (8, 300) 


f  0  1  =  J .  0 

t  n  s «  o .  o 

HO  - .  1 ?s 
to=o. n 

Y4M A  X  0  = -  l 
YSMI  >40*1000. 

Y  8  M  A  X  0  =  1200. 
Y2M AXO= .  1 

Y  5  N  (J 2  1  1  CO. 
NRR=  1 

NFU  -  1  2 

p(  l )  =  -  .  nsjr 


TEM00070 

TFV00080 

TFM00090 

r T  E  v00  l  00 
TEVOO  1  10 


TFMOO  120 
TFM00130 
TEMOO  140 
T  F MOO  1  SO 
TEMOO  160 
TE  MOO l 70 
TEMPO  1 RO 
T  E  MOO  1 90 


lc ,41 , ASEOU, XP1 ,XP2 ,XDl ,XD?,X03,XR  1, XR2, MUDE , 
SIGMA,NR,MUDER,MCUEP,MMGDEP, IOEL, TIMCEL , NP,M2,M3, 
XP3t L Sb, TDl , T03, Y4MAX0, YbMl NO, Y5MAX0 , Y2MAX0, YbNO 
, X  P  4 ,  IDELI  ,  INP 


T  E MOO  200 
TFM00210 
T  F  M  00  2  20 


TFM00240 
T  E  M  0  0  2  b  0 


TFM00280 

TFM00290 


T I r 00 300 
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o  o  o  o  o 


P( 2  )=37.D0/’4. 00 

TEM 00310 

P(  i)=-57. 00/24. 00 

TE  MOO  320 

P(4)=5S. DC/24. 00 

T  F  MQO  3  30 

C(  1  1  =  1. co/24. oo 

TEMOO  340 

C  (  2  )  .  00/  ,4  .  L)0 

TEM003b0 

c ,  3 )  =  i  ).no/  "’4. oo 

TEM00360 

C( 4  )  =  .  ,  7SD0 

TEM00370 

0PLGA0  =  ( 2 .00+00) *P I / ( 24. 00+00* 3. 6T+0 3) 

T  E MOO  380 

c 

T  E  MOO  390 

c 

oec.  inning  or  i  ntut  loop 

T  E  MO 0400 

C  FOR  PFIL  SMITH  r HE  UNITS  OF  Thfc  INPUT  HERE  CHANGEO  TO  DEGREES 


10  READ  (b*PtTr,tND=70) 

C  AND  FUK  THE  W H E p L  SPEFO  TO  K PM 
I NUL  X*0 
KAD  =  b7.2‘)b7° 

D(J  7  GO  ISP=l,4 
7  b  0  Y (  I SP )  =Y0(  I ) /KAO 
Y (  b  )  =  Y0( b) * . 006  80b  b 
Y  0  b  G  =  Y  (  b  ) 

DU  7  j l  IPS  =  7  ,N£C 
7 b 1  Y (  1  P  S  )  =  Y  0  (  I °S ) / RAO 
Y4MAX  =  Y4M,AX°/RAD 

y?max=y2max°/rad 

YbMI  ^  =  Y 5M.  I  N°*  •  006  80b  b 
Y bM A X  =  YbM A X°*.  01)68055 
YbN=YbNO*. 0°680bb 
IF  (TIN)  bO  TO  70 
IE  (  TOO  )  GO  TO  30 
READ  ( b  *  2  0  )  TO, YU 
20  FORMAT  (71b) 

DU  60  1  =  1,  NEC 
bO  Y (  l  ) =  Y 0 (  I  ) 

GO  TO  40 
10  T0=0.00 

40  READ  ( b i b  G  )  TI  TIE 
bO  FORMAT  (  I  C A °  ) 

INITIAL  ANGULAR  MOMENTUM 

X  =  Y ( 5)*(Y(6)*YI4)  +  Y(B)  ) 

FtSQO  =  (  XJ(  1  )  *Y  (  6  )  *Y  (  b  )  *OCOS  (  Y  (  4  )  )  )**2  +  XJ(2)*Y(7)**2MXJ(  3  )  ♦  Y  (  H  )  - 
1  Y(b)*0MN(Y(4)  )  )**2 


XJ2i  =  XJ( 2  )  -  v  J  (  3)  T  F  M  0  0  b  7  0 

XJil  =  XJ(  1)-YJ(  1)  T  F  M  0  0  b  8  0 

XJ21=XJ(2)-YJ(  1  )  T  E  MOO  b90 

TEM00600 

H=hO  TFM006I0 

r=TO  TF  MOO  6. 20 


T  E  MO 04  30 
TE MO 04 40 
TEM004S0 
TEM00460 
T  E  MO  07  30 
TEM00640 
T E  M  Q 0 4  70 
TEM00480 
TEM00490 
TEMOObOO 
TEM 00510 

TEM00b?0 


T  I  M  *  T 
I  P  R  N  =  0 
18  =  0 

CALL  TD<K(71,  U.) 

WR  =  (  T  MOUND- T  0 ) / LO.DO 
WRITE  (6, PETE) 

WRIT,:  (6,61)  TITLE 
7,1  POKE.  A  I  (  1  l  *  *  l  6  A  8  ) 

PEGIN  SI 3ULAT 10* 

1  EH  D= . F  At SE. 


TEMOObbO 
T  F  MOO  b60 
TEM00670 
TEM006R0 
TFMOOb^O 
T  E  MOO  700 
TPM  00710 
TEMOO  720 


S  (  1  )  =  0 . 0 
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S(2)-0.0 
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CALL  OUTPUT  t  TfY,  TERM 
CALL  T I “HR ( T I  Ml  l  ) 

CALL  RK ( H,  T , Y  , FYf TERM  ) 

CALL  AM ( H  « I ,Y,FY,GLB,LUb,TCkM) 

CALL  TP*HR( TIME?) 

F  L  T  l  M f  =  TP*  E2  -  TlMhl 
I  TIME  =  (LLTIML/NSTLP)*3600.0 
FLT  l  Ml  =  F  L  t  l  M  6  *  6  0 . 0 

wm  IF  (6,62)  I  Tin,  t  L  T  IMF 
62  FORMAT  (  f  tLAPSFO  TIME  FUK  UNf  ITLRATICN  = 
2  U  L  A (  l L  M  I  I M  r  =  »,  F  7 . 6  ,  f  MINUTE  S  *  ) 

*RI Tfc (6,64)v( i i ) 

WRI TE(6,6S)  Y ( L2) 

FORMAT  (  /  /  *  n  F  R  F  0  R  M  A  *4  C  L  INDICATFOR  FCK  PITCH 
66  FOKMAT(//f  nLRFORMANCF  INDICATOR  FUR  ROLL 


TFM00730 

TEM00740 

TFM00750 

TEM0076O 
TEMO0770 
TEMOO  780 
TEM00790 
TEM00800 
TEM00810 

’,E7.6,»  SECONDS  SIMTFM00820 

TFM00830 


AXISO  1  ,  G 1 0 • 3 ) 
AXISO  * , G 10 • 3 ) 


C  PLOT  iNTtURAL  COVES. 

VsRlTE(8|63MY0(  I), 1  =  1,11), HO,  TO,  T  BOUND,  XJ 
63  FORMAT (  f Y0  = • , 1  1  ( F9. A,  *  ,*),’  •  ,  * HO- • » f 6 .  i  , • ,  T*ffF6.1,f 

L  F  6 •  L ,  *  SCC.  ,  J  =  *  »  3 ( G l 0 . 3  f  )  , * J A= *  , U 1 0  .  3  ) 

R  F  A I  /  (  H ,  t,  7  )  L  A  b  I.  L 
6  7  FORMAT  (40AM 


C 


f  L  P  SLM  VS  Tf, 0,1007) 


LPSLN2  VS  T •  ,0,1007) 


•F.PSLNi  VS  T  •  ,0, 100  7) 


CALL  FRAMLVfO) 

CALL  °kINTV( 128, T1 TLE, 0,976) 

CALI  PRINTVf  124, LABI L, 0,806) 

CALL  PR l NT Vf -  14 ,  ’INITIAL  VALUL$’,60C,1010) 
CALL  P R I N T V ( 72 , LABEL (  32)  ,0,866) 

CALL  GRl(NFf>,PT  ,PY1  ) 

IF  (  IERR.LJ.01CALL  PRINTVC-ll 
CALL  GR 1 ( \F  M  PT , PY2  ) 

If  ( I f R7.L0.01CALL  PftlNTV(-ll 
CALL  GR I(NFn,Pf,PY3) 

If  (  I  L  R  R  .  L  0 . 0  )  CALL  PRIMV(-11 
CALL  GR I (NF  ' ,PT, PY4 ) 

IF  (  IFRR.L J.OJCALL  PRlNlV(-ll 
CALL  OR l ( NFP , P  T , P Y  6 ) 

If  (  I  f RR.t J.O)CALL  PKINTVt  —  14 
CALL  GR 1 ( \f* , P T , PY 7 ) 

IF (  I  ERR. FU.^) CALL  PR  I  NT V ( -4 , * Y ( 6 ) 

CALL  GRl(\f°,PT,SP( 1,1) ) 

IFMCkR.FC.MCALL  P  R  I  N  T  V  (  -  l  3  ,  ’SENSOR  1  VS 
call  GRl(NFn,PT,SP(  1 ,2)  ) 

IF(UR.EC.O)  CALL  PR  l  NT  V  (  -  1  3  ,  f  SENSOR  2  VS 
NPL  = I NDlX 


TMFTA  VS  T1 


•^HEEL-MOM  VS  T 


,0,1007) 


100  7) 

,0, 1007) 

T  *  ,0 ,  1  00  7 ) 

T  *  ,  0 ,  1007) 


IF  (MIJCFP.L0.2JCALL  GK  1  (  NF  R , X  T  ,  Y  T  ) 
CALL  FRAMtV(O) 

CALL  PRINTV( 128, TI TLt  ,0, 776) 

CALL  PRPJTVf  124,  LABEL,  0,896) 

CALL  PRINTV( 72,LABEL( 32) ,0,B6S) 


TO 


NPL  =0 

TT  IME  ^  l 
AFRl  =  l  .  0  °  ♦  C  3 
APOI  4T  =  C 
NS  TIP  =  0 

i'iO  Tj  10 

70  CJ  4  T  I  40- 


TEM00840 

TEM00860 


TFMOOQOO 

TFMOOOIO 

TFM00020 

TFM00030 

TFM00040 


TFM00060 
TFMOOOOO 
TEMO 1020 
TFMO  l(  60 
TFM01C80 


TFM()  l  100 
TFMO  1110 


TEMO  l  l  30 
TFMO  l  I  40 
TFMO  1  160 
TFMO  1  160 
TFM01  I  70 
TFMO  l  180 
TFMO  1  1 90 
TFMO l 200 


CALL  PLTND 
STUP 
F  NO 


TEMO1210 

TLK01220 

TEM01230 
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on  noon 


SUBROUTINE  PERIV(Y«YDOT) 

L  2  3  A  67891011 

Y=EPSl,EPS2,EPS3,  THETA,MHECL-M0M,0M1 ,GM2,0M3,X1  ,  X2,X  3 

IMPLICIT  Rt*L*8  IA-H,U-Z) 

TRWULS 

PLAL*B  M.  2  ,  M 

KE  AL  *8  LI,  12,  L  3 

DIMENSION  Y (  1 ) , Y  00  T ( 1 ) , X  J ( A ) 

COMMON  /  A  L  I r'  N  /  M2, M3 

COMMON  /ELL  FN/  TIM DEL, TIM, SIGMA, I B  ,  NP , MODER , NR , I RNP, IRNR , MOOEP, 

2  I  DEL , MMCDEP, IPNP,XP3,LSF 

COMMON  /PAK*M/XJ,XJ23,XJ31,XJ21,TL, T 2 , OMEGAO , XP 1 ,XP2,XDL,XD2,XD3, 
2  XK 1 ,  X  R  2 

2  /CON  fR  L /  LL,  L2,  L3 

COMMON  /DISTUB/  TO l , TD3 
RLAL*4  SP( 6n0l , 2) 

COMMON  /BETTER/  S  (  2  )  ,SP 


OFF  I  NT  I  ON  Of  CGNTRUL  VARIABLES 


c 

NAME 

0  E  S  C IPT  ION 

SYSTEM 

INI  TS 

c 

XP  1 

P'>UP 

PI  TCH 

SEC.  D  -1 

c 

x  r  2 

IMTEuRAL 

PI  TCH 

SEC.  D-2 

c 

x  D  l 

DrK I V  A  1  1 Vt 

DAMPER 

NONE 

c 

XD2 

P°OP 

DAMPER 

SEC.  n  -L 

c 

XP  l 

P^OP 

ROLL 

SEC  D  -1 

c 

XR2 

I  v  T  E  G  R  A  L 

ROLL 

SEC  0  -2 

L 

T  1  = 

.06  4  7-.4  4  6U-  3*Y(6)/XJ(4) 

T  ?  =  - .00  10  7 ♦.66  2  70- 4* Y( 5 ) /X J ( A ) 

C Y l  =  UCOS ( Y(  n  ) 

SY1=0SIN( Y( 1 ) ) 

CY2=DCUS( Y( ?) ) 

SY2- DS I N I  Y(  *»)  ) 

C Y  3  =  L)COS  (  Y  (  M  ) 

S  Y  3  -  D  S  I  N  (  Y  <  ^  )  ) 

TY?=SY2/CY2 

SI  1  ) =  0  A  T  AN  2  I  SY  1*CY3*CY1*SY3*SY2,  CY1*CY2) 

SI  2  )  =0 A  T  AN2 ISY2*CYl*CY3-$Yl*SY3,CYl*CY2) 

IP= I  6.283 18^00) /( Y( SI/XJ <4 ) - Y  < 6) +Y( 8 )*Y( 4) ) 

I PNP= I  DINT  I  TP*DFLOAT I LSH ) ) 

IT  I  I  ft • EG • 0  )  T  BE  =0 

CALL  DIBIT  (Y(4),Y(6),Y(  10)) 

IF!  ItJE.NF.O)  GO  TO  LO 
1  HE  =  10 
GO  TO  11 

10  I  r  I  I  T  I M- TL Ac T  )  .L  I  . T AU)  GO  TO  IS 
L  I  ILKKUK= IPNP-IRNP 

TL  AST  =  T  1M 
T  AU= r P 

IS  CONTINUE 

THE  DOT  -  l-L. nDO/XDl)*(X D2 *Y(4)4Y(5)*(Y(8)4(Y(4)+ M2 )»Y(6) 

1  -M3*Y( 7) *( Y( A  )  +M2  )  ) 

2  -O.SBaO-CA^DFLGAT (  I R NR  )  ) 

HTML  T  =  Y  (  S  )  *  I  T HLDU T  4  Y I  7 )  ) 

C  MOT  GP.  TOROUE 

TM=  T  L 

IF  I  ILKRCK.L*  • C • 0 D 0 )  T M *  —  T 2 
C  FIRST  ORDER  CUL‘ w  ANGLES 


TEM02620 
TEM02630 
TEM02640 
TFM026S0 
T  F  MO  2  66  0 
T  F  MO  2  6  70 

T  EM 02680 
T  E  MO  2  690 


T  E  MO 2  7 00 
TEM027L0 
TFM0272O 


TEM02730 
TEM 02740 
TFM02750 
TFM02760 
TFM02  7 70 
TFMO?  780 
TEM02790 
TEM02810 
TEM0282O 
TEM02H 30 


TFM02P70 

TEM02890 


T  C MO 2920 
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YDO  T (l)=(Y(')*CY3-Y(7)*SY3) /CY2-QMECAO 

Y  DO  T (?)=Y(f>)*SY3*Y( /) *CY  3 

YDCl ( 3)^Y(8)-TY2*(Y(6)*CY3-Y(7)*SY3) 


C  ROLL  AXIS 

YOU!  ( A )  =  T^bDll  T 


C 


C  P  I  T  CH  *iXIS 

YfJL)T(S)  =  T" 


YLUT ( 5 ) 
KAILS 


TEM02960 
TEM02970 
TEM02980 
TEM 02990 
TEM 03000 


H  l  =  L  1  ♦  T  D  I 
H }  =  L 3  ♦  T  0  3 

YOU  r  (  0  )  s  (  H  1  ♦  X  J  2  3* Y  (  7  )  *Y  (8)  ♦  (Y(  4  )  +  !»2  )  *HTHCT-TM-M3*Y  (  5  )  *Y  {  &  )  )/XJ(  1) 

Y  DO  T  (  7  )  =  (  L  2  ♦  X  J  3  l  *  Y  (  6  )  *  Y  (  8  )  ♦  M  3  *  1  M-  Y  (  5  )  *  (  (  Y  (  4  )  ♦  M2  )  *  (  Y  (  6  )  ♦  M  3  *  T H fc  DU  T  )  ♦ 

I  Y ( 8 )  )  )/Xj(2> 

YOGT ( 8 ) = ( H3-XJ21*Y( 6) *Y( 7) +HTHET* { Y (4 ) *M2 ) *TM+M3*Y( 5 ) *Y ( 6 ) ) /XJ { 3 ) 

C  PITCH  AXIS  CONTROL  L  Ak  TEM03C40 

YOU  I ( 9 ) =Y (  M  TEKO 3050 

YOGI  (  10)  =Y(  M-GMtGAO 
YOOT  (  I  I  )  =l'A^S  (  Y  (  I  )  )  *  T  I  V 
YOUT  (  I2)  =  UA',S(Y(2) 

RETURN  T  E  ^0  3060 

ENL)  TEN03090 
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SUBROUTINE  p  I  G  I  T  (Y4.Y5.Y10) 

IMPLICIT  REAL*8  (A-H.U-Z)  0IG00020 

DIMENSION  IrAMP(  16)  ,  ISAMRt  16) 

INTEGER  CNPMl  N  ,  CNRM  I  N,  ON  PM  AX  ,  CNRM A  X  DIG00040 

COMMON  /ELLrN/T  I  MO EL , T , S  IGMA1 , I B, NP , MCDER , NR , IRNP,  I RNR , MODEP,  IDCL  * 

IMMOULP, l PNP «  X  P  3 i L  S  D 


CDMM(JN/PARAr/XJ  ( 4 ) , CUM ( 6 ) , XP 1 , XP2 . XD 1 .  DUMM  (  2  )  , XR  1 , XR2 
RCAL*4  SP ( 5nO  1 »  2 ) 

COMMON  /bLT  TER/  Y (  2 )  , SP 
RLAl*4  XT,YT 

COMMON  / P  L  J  T /  XT! 5000)  , Y  T ( 5000) 

COMMON  / T  R  C  n /  YC5C  *  INDEX 


0  I  GO 00  70 
01 GOOO0O 
0  I  G  0  0  0  9  0 


COMMON /S ECU /OUM 2 ( 3) , I SEOU( 3) 

COMMON/ PI TCU/XP4, I  DELS •  I NP 
COMMON  /COEr/ZZX ( 8 )  ,  T60LND 
EQUIVALENCE  ( XJ4,DUM( 4)  ) , (ZP1 «XP1) 

DATA  DEL  TAT  / 0 . 2 5D0 / , DL L  T AD / 0 . U 1 L/,0FLTAR/0.  1920- 
DATA  EPS/1. r-12/»F0V/O. 17453DO/ 

COMMON  /ELLEN/  APPEARS  IN  MA  I  N , AM , DKR I  V f RK 

SUBROUTINE  OIuIT 

SIMULATES  DIGITAL  CONTROL  ON  ROLL  AXIS  AND  CN  PITCH  AXIS  OE  LCS7 


C  LOOP  PRCCfSSSlD  AT  INITIAL  CALL  TC  DIGIT  FOR  EACH  SET  OF  INPUT  DATA 
IMIB.NE.G)  GC  TO  1C 
TZFRUM 
N  =  0 
I  S  =  G 


01  GOO  100 
0  I  GOO  1  1  0 

3/ t KR AN/ 156 2 78590/ 01  GOO  120 
DIG 00130 
DIG 00140 
0  I  GOO  ISO 
0  I G 00 1 60 
OIGOO  1  70 
OIGOO  180 
OIGOO 190 
0 I G00200 
0 1 GOO  2 1 0 
0  I  GOO  2 20 
01  GOO  ?  30 


l S  PO-O 
IB  =  10 

TWCiP  1=6.2831  85300 

C  CONVERT  SIGMA  Tf’  L  SB  UNITS  DELTA  =.011  DEG 
C  CALCULATE  P^U8An IL I TY 
C  SIGMAI  IS  FUR  A  4  SLC  RMS 
S l GM a4  =  S l GM 3  I *4 
S l GMA=S l GMA4/0LL l AD 
P=( SIGMA )**?/2.C4 
XL  SB  =  DFLUA  T ( L  SB ) 

IIN1  =  IDINT(I6.2P  3 1 8  500* X  J ( 4 ) *XL  S3) / C YU5L  *  XP  2  ) ) 

I  GNP-  I  01  NT  (  I  6.  28il85l)0*XJ(  4  )  ♦  XL  SB  I  /  (  Y5  *XP?  )  ) 
CNPMAX=I0INT( ( ( 6. 823185 D 0*9. 5600+  XLSB)/6.D2)/XP2) 

(JNPM  IN  =  10  INU  ((  6.2  8  318  500*9.  5500*  XL  SO  )  /  1  3 . 02  )  /  XP2  ) 
QNRM  IN*I0INT(-2.0**10./(XR1*XR2H 
CNRM  Ax  =  I  At'3S  I  CNRM  I  N  ) 

RMAX=2.D0**I 0.00 
IQNR-0 
l  RNR- 0 

IRNP=ICINT(  (DFLUATI  I  GNP)  )*(XP2)  ) 

I  p  3  -  0 

DEL  T $  =  3. I  4 l 6926/4096.00 
I SSP0=0 
I  RNP1 =0 
I  RNPC -  I  PIMP 
GC  TO  10 

c 

C  TEST  FUR  PICKING  UP  SEMSUR  UU1PUT  IN  SUN  ACQUISITION  MCDF 

C 

10  If  (mj^FP.NL.  3)  GU  TU  109 


01 G 00240 

0  I  G  0  0  2  5  0 
D  I  GOO 2 70 


0  I  G  0  0  3  3  0 
D  I  G  0  0  3  4  0 
0 1  GOO  3  70 
0  I  GOO  3  80 
OIGOO  390 
01 G00400 
D  I  G  0  0  4  1  0 
0  I  G  0  0  4  3  0 

D I  GO 04 4  0 
D  I  G  0  0  4  5  0 
(PITCH  A  X  I  SOI  GO  04  60 
0  I  G  0  0  4  7  0 
01 G00480 
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IF  t  IP  3.EQ.01GC  TU  IC9 

IF  (  r-TP  3  .LT  .  r  IMDtl  )  GCl  TO  109 

I  P  5  =  0 


I»NP= IRNPT 

c 

L  ***  r  t  S  T  TO  SEE  IF  LLAPSFC  U ML  IS  1/A  SEC  *** 
C 

109  I  F (  T-TZcKG.I  T.OELTAT )  RETURN 

l  S  = I S+  1 


c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

u 

c 

V. 


DIGITAL  CONTROL  LCOPS  —  EVERY  l/A  SEC 
1.  ADJUSTABLE  GAIN  PARAMETERS 

2  ADJUST  Abt  t  INTEGRATOR  SAMPLE  PERIOD  AND  SATURATION  LOOP 
5  SENSOR  E  nL  D-UVER  CH Ak AC T E R I S T I C S 
A  SENSOR  Nr I S  E 

5  MUDE  SWITCH  —  PITCH  AXIS 

MGOtP  =1  CONSTANT  SPEED 
MOOEP  = 2  POINTING 
MCOEP  - 3  AC  CO  I S I TIGN 

6  INTEGER  ARITHMETIC 

7  MICE  S  W  I  T  C  H  —  ROLL  AXIS 

MOOER  =  1  DAMPING 
M  0  0  E  R  =  2  POINTING 
MCDER  =  3  GIMbAL  CONTROL 


U 

C  ******RuLL  AXIS  CONTROL  SECTION 
C 

GO  (O  (  I  9  t  2  ° i  19) *  MODEM 
21  PMSR-0.0 


C 

c  gfnlkatl  random  number 

C 

IT ( SlGMA+tPS.GE.O.DO.ANC.SlGMA-LPS.LE.O.CO)  GO  TO  129 
DO  119  1=1,100 

IF ( RAN 2 ( KRA  )  . L E . P ) RM SR  =  RN SR ♦ 1 0 . 

119  I  f-  (  RAN?  (KRA‘  )  •  L  L  .  P  )  RM  S  R  =  R  M  S  R-  1  C  . 

C 

C  FOLD  TVER  SENSCT  CHARACTERISTIC  ANO  FIELO  OF  VIEW 
C 

129  I  SAMR  (  I S )  =  I °I N  T  (  Y(  ?)/DELTAR+RMSR) 

YMGD=DMUD(Y( l ) , TwOPI ) 

I  F  I  EARS  (  YMQP  )  .  (,  T  .  FUV  )  I  SAMR  (  I  S  )  =0 

IF (  I ABS (  l SA“R (  1 S  )  ) .Gfc. IG2A . ANO-  I ACsSI I SAMR (  I S ) ) .LT. 20A8) 
1  I  SAMR  (  IS)  =  I2CArt-IAi3S(  I  SAMR  (  IS)  )  ) *  I  SIGN!  1  ,  I  SAMR  (  IS)  ) 

IT  (  I  A  H  S (  I  S  A  “  R  (  l  S  )  )  .  G  C  . 20 A 8 ) I SAMR  CIS) =0 


C 


******  PITCH  A  X  *  S  CCNTRCL  SLCTION 


19  GO  TO  I  IH,2°,  18,2R,2b)  ,MODEP 

c  mode p= i  rate  integrating  gyru  simulaticn  eor  pitch  axis 

2  4>  I  S  A  f  *  P  (  IS)  =  IpINT(Y10/0tLTAR) 


0  I  GO  DA  90 
OIGOOSOO 
0IG00510 
0 1  GOO 520 
D I  GOO 6  30 
D I G005A0 
0  I  GOO 5 SO 
0  I  G  0  0  S  6  0 
0IG00S70 
DIG00S80 
0 1  GOO  5  90 
0IGDD60D 
0 1  GO 06 1 0 
01000620 
D  I  G  0  0  6  3  0 
0IG006A0 
DIG006S0 
D I  GO 0  6fe0 
0  I  GO  0  6  70 
01 G00680 
0  I  GO 0 690 
0  I  GOO  700 
0  I  GO 0  7  1  0 
01  GOO  720 
U I  GO 0  7  30 
0 IG007A0 
0  I G  0  0  7  5  0 
0  I  GOO  760 
OIGOO  770 
01  GOO  780 
OIGOO  790 
01 G 00800 
0  I  GO 08 1 0 
0  I  G  0  0  8  2  0 


01 G00850 
D  I  G  0  0  8  6  0 
DIG00870 
DIG00880 

D I GOO  890 
0  I  GO  0900 
D  I  G  0  0  9  1  0 
D I  GOO 9 20 
0  I  G  0  0  9  3  0 
0  I  GOO 9 A 0 
0  I  GOO  9 SO 


C 

c 

c 

c 

c 

c 

c 


GG  T  (j  18 
ND  l  ST  GLiSiER  A  TCR 

INPUTO  S  I  GM  *  (OSLO  TO  CALCULATE 
SPEC l E  Y  SENSOR  RMS  NOISE  LCVEL  IN 
OUTPUT  1  RMS 

DIGITAL  NUMBER  WITH  ZlRT  MEAN  AND 
TO  SAMPLED,  QUANTIZED  PITCH  EKRCR 


P  EIRST  TIMF  THROUGH) 
DCGRELS  ECR  ONE  SCAN  0  SIG 


RMS  IS  APPROX. 


0  I  GO  0 9  70 
DIG00980 
0  I  G  0  0  9  9  0 
0  I  GO  l  COO 
0  I  GO  1 0 l 0 

SIG  EUR  A D C I T ION  DIG01020 
OIGO 1030 
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c 

28  R  M  S  •=  0  •  0 

IFt SIGMA+EPS.Gt. 0.00. AND.  SIGMA-FPS.LE.0.D0)  GO  TG  12 

C. 

C  GENERATE  RANDOM  NUMhFR 
C 

Dll  II  1  =  1,  1^0 

I F ( ft AN 2 ( KRAV ) .LE .P  J  RMS=RM$*10. 

11  IF ( R  AN  2  t  K  R  AM ) . L  E • P )  RMS=RMS- 10. 

12  Y  M  G  f  J  - D  M 1  j  0 ( Y (  1 ) , TWOPI  ) 

ISA^PC  I SJ^I^INT ( YMOD/OLL  TAR  +  RMS  ) 

C 

G  FOLD  CIVlR  ScNSO°  CHARAC  TER  I  S I  T  I C  AND  FIFLD  OF  VIEW 
C  FIELD  OF  VIEW  LIMITATION  ON  ROLL  ERROR  FOR  PITCH  SENSCR 
I  r  CDAbSC  Y ( 2 )  ) . G  T • F  U  V )  I  SAMP (  I S 1=0 
C  FOLD  OVER  CHARArTERI ST IC  WITHIN  FGV 

IF(  I  A  R  S  (  I  S  A  “  P  (  IS)  )  •  G  L  .  1G24.ANU*  IABSt  ISAMPt  IS)  J.LT.2048)  ISAMPt  I  S  )  = 
1  ( 20 A o- I ABS(  ISAMPt  IS)  )  ) *  I S I GN ( 1 «  I SAMPt  IS)) 

I  F (  IAPSt  ISAMPt  IS))  .GL. 2048 )  ISAMPt  I S ) =0 
C 
c 

C  **********  C  H  t  C  M  TO  SEE  IF  ELAPSED  FIVE:  EQUALS  4  SEC.  ♦♦♦****♦** 

C 

18  T/LKU=I 

(F  t  I S.LT. 16)  RETURN 
N  =  N*  1 
C 

C  ***  ROLL  AXIS  **** 

C 

GO  Tu  t  39, 4^, 59) , MODEM 
49  I  SR  =  0 

RU  169  1=1,16 
169  t  SR= l SR* I SA>  R (  I  ) 

I  SR  = I  SR/  16 
GO  TO  110 

59  ISK  =  — IOINT ( Y4 / DfcL  T AR  ) 

110  (RNR* IDI NT  t  OFLOAT ( I  SR* I D I NT ( DEL DAT  t l QNR ) *XR2)  ) *  XR1) 

IF  tMJOt  >l,NR).EG.O)  I QNR *  I ONR *  I  SR 
I F (  I GNR.GT.nNRMAX)  I QNR  =  GNRM A  X 
IF ( I ONR .LT.ONRMIN) IQNR=CNRMIN 

C 

C  SATURATION  FUR  ROLL  AXIX  R  NR 
C 

RTEMP  =  OFL(JAT  (  I  R NR  ) 

IFt  IAHSt  IRN* ) .GT.RMAX)  I  R  NR*RM  AX  *DS  I  GN  (  1 .  ODO  ,  R  T  E  MP  ) 

GO  TO  69 

19  I  R  N  R  -  0  •  0 
C 

C  ****  PI T C H  AXIS  CONTROL  SLCTIUN  **** 

C 

69  GO  T  j  t  36 , 4  ° , 56  » 78,48)  » MCDEP 
6  8  YMOD  =  nMCOt  Y  t  1  )  , TWCP I  ) 

ISSP=IDINT( vmOD/DELTS) 

I  SSP  =  -1*I S$° 

I  AS  = I  A  6S (  ISrP) 

IFt  (  AS.Gl .  ln24 . AND.  IAS. I  I. 2048)  ISSP=1024*ISlGNtl,ISSP) 

IF (  (  \S.GE.2°4P )  I SSP  =  0 
I  0 A  P  = (  (SSP-*  SSPU)/4 
I SSPO= I SSP 


D I  GO  1040 
DIG01C50 
D I  GO  1 060 
D I  GO  1 C  70 
DIGOIOHO 
D1G01C90 
0  I  GO  1 100 


0  I  GO  1  130 
DIGO 1  140 
01  GO  1  150 
DIGO  1  160 
DIG01  170 
01  GO  1  I  no 
D I  GO  1  I  90 
01001200 
01  GO  1210 
0  I  GO  1 220 
0  I  GO  1 2  30 
0  I  G  0  1  2  4  0 
DIGO 1260 
01  GO  1 260 
0  I  GO  12  70 
0  I  GO  1280 
0  I  GO  1 290 
01 GOi 300 
DIGO  1  no 
DIGO 1 320 
01  GO  13 30 
DIG01  140 
0  I  GO  1  160 
DIG01  360 
DIG01 370 

0 1  GO  1  390 
0  I  GO  1400 
DIG014  10 
DIGO  1420 
DIGO  1  A  30 
01  GO  144  0 
DIGO 1460 
DIGO  146 
DIG014  70 
01  GO  14  80 
DIGO  1490 
D I  GO  1 500 
0  ICO  1510 
DIGO 1620 


01  GO  1 640 

0  I  GO  l  8 *>0 
01 GOI 660 
0  I  GO  15  70 

DIG01 6  70 
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or  oo  —  o  o  r 


0  I  GO  1 600 


I  S|  ti  =  I  OAP*  I  'M  NT  (  XP  3  )  ♦  I  SSP 
I  F  (  I  AtJS(  I  S  I  r  )  .UT  .  I  DEL  )  GO  TO  56 
I R  NP  T  = IKNPC 
GO  TO  57 

*5  ci  I  R N P T  =  M Y U Dfc  n  *  I  SIGN  I  1  »  1  S  I  G  ) 

l R  N  PC  -  I PNP 
5  7  IP  3=1 

t  p  3  =  r 
go  t;j  68 

AVERAGING  LOGIC 


I  S  P  =  0 

GO  15  1=1,1' 

I SP= I SP  ♦  I S A"P (  I  ) 

I SP^ I SP/  16*1-1) 

INUL*X=  INDEX*  1 
XT  I  INDEX )  =  T 
VT I  INDEX )  =  I  CP 

INTEGRATOR  SATURATION  LOGIC  -  PITCH  AND  ROLL 

Tic  SATURATION  LOGIC  LIMITS  THE  INTEGRATOR  CONTENT  TO  NUMCERS 
KlPRtSLNlMr  WhclL  SPEEDS  OE  90 0  TO  1  300  RPK  APPROXIMATELY 
C  The  UHL1L  SPEED  R  E  G I S  T  L  R  CONTENT  REPRESENTS  ONE  FOURTH  CE  A  DESIRED 
C  REVOLUTION.  PERIOD  RtSULVEO  TO  0.1  MSEC 

C  UPPER  THRESHOLD  1  300  R PM 

IRNP*(  (  I SP) *IDI NT ( XP1 )  +  I  DINT (  (DEL OAT (  I UNP) ) *1 XP2 ) ) ) 
ir  I  MODI  v»,'Nn  )  .EG.O)  UNP=  IQNP*  I  SP 
IF  I  I  JNP.GT.PNPMAX )  ICNP=CNPMAX 
IT  I  IUNP.L I  .°NPMIN)  ICNP  =  ONPMIN 
l  VJ  M  M  QNP-  IP  T 

C  HR  I  TE  (  6, 61  OT  ,  I  SP,  I  HM 

61/  FORMAT (  1 X  » '  T  I  M  L  =  •  ,  1PL13.6, •  SECONDS  *  , •  I SP = 1  .  I  6 ,  *  I CN P= * ,  I  6 , / / ) 

GO  TU  68 

C  MOO E P  =  4  IS  THE  ° I TCh  LACK  UP  MODE 

78  IHMUDIN,  IN°).NE.O)  uO  TO  6d 
I  S  P  =  0 

DO  79  1=1,1/ 

79  ISP-I SP * I S  A  *'  P  (  I  ) 

I SP=I SP/ 16 

I  DA  P  G= (  I S  P- f  SPO  )  / (  1 N P *  4  ) 

I  SPO=  I  S'1 

I  S  IG8*  I  DAPti*  I  CENT  I  XP4  )  ♦  I  SP 
IF  I  ISIG8.GT.  IDELB)  ISEOUm*r) 

I  E (  I  S IGH.L T 1* IDELR ) I SEQUII )  =  1 
I  E  (  I  APS  I  I  S  I  r-H  )  .  LE.  I  DELB  )  I  SLCL(  L  )  =0 
IRNP= I  INI *Xn2 
68  I  S  =  U 

13 F  TOR N 
I  NO 


0  I  GO  1 64  0 
D I  GO  1  650 
D I  GO l 660 
D I  GO  16  70 
D I  GO  16 HO 
D I  GO  1 69  0 
D1G01 700 
0 1  GO  1 7 1 0 
D 1  G  0  1  7  2  0 
01  GO l 7  30 
0 1  GO  1 740 
DIG01 750 
0  I  GO L  760 
D I  GO  1  7  70 
D1G01 780 
D1 GO  1790 
01  GO  1  Ron 
D 1  GO  1 8  1  0 
0  1  GO  1  R  20 
0! GO  1830 
DI GO  1840 
D 1  GO  1850 
0  I  GO  1 860 
D I  GO  10  70 
0  I  GO  18  80 
0IG0189O 
01  GO  L TOO 


0 1  GO  1  920 
0  I  G  0  1  9  3  0 
Dl GO  1940 
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ooo  o  o  o  o  o 


suhkuii t  i n  t  nurpur  (  t  ,  y  ,  t  e  r  m  ) 

IMPL  I  C  (  T  Kt/,L*8(  A-H,0-2) 

DIMENSION  Y  f  L ) 

REAL *4  PX  ( 5001  )  ,PY  I  (  boon  f  PY2I  *>001)  ,PY3{  5001)  ,PY4{  5  CO  l  )  ,PY5(  5001  ) 
R  E  A  L  *  4  SP(5n0lt2) 

RE  AL  *  4  PY7 ( ^COl  ) 

COMMON  /  TE  Mn  /  P  Y  7 
COMMON  /BE  T  TER/  S(  2  )  ,  SP 
LOGICAL  1  E  R  v 
0 COMMON  /OR  /nliM  (  3  )  i  NPL 
L  /OTPT/PXfPYl  »PY2,PY3,PY4,PY5 

DATA  RAC  / .^l 74532925/ 


Nj  P  L  =  N  P  L  ♦  t 


T EMO  3  990 
TEM04000 
TEMO40  1 0 
TEM04C20 


TFM04030 

TEM04C40 

TEM04C50 

TEM04C60 

TEM04070 

TEM04080 


PICK  UP  The  INITIAL  UHEbL  SPtED  BIAS 


IF ( NPL •CD. 1 ) WHSDO=Y { 5 )/. 006 0055 
IF  (NPL. LT. *>001)  GO  TU  20 
TERM- . TRUt . 

UR  I TE  (6,10) 

L  0  FORMAT  (»C  °LOT  ARRAYS  FILLED  COMPLETELY.*  ) 
20  PX (NPL  )  * T 

PY  L ( NPL  )  =Y{  1) /RAO 
P Y 2  (  NPL  )  =  Y  {  •>  )  /RAD 
PY  5( NPL  ) =Y(  3 ) /RAD 
P Y 4  (  NPL  )  ^Y(  '■  )  /RAD 
SP ( NPL , 1 ) =S( 1 ) /RAO 
SP(NPL,2)=S(2)/KAD 

SUBTRACT  THE  INITIAL  UHELL  SPEED  BIAS  UHSDO 

PY6(  NPL ) -Y( r) /.0068C55-WHSD0 
PY  7 (  NPL ) =Y( 6 ) /RAD 
RE  TURN 
END 


TEM04C90 
TEM04  TOO 
TEM04 I  10 
TFM04 I  20 
T E M 0 4  l  30 
TEM04  140 
TEM04  150 
T  EMO  4 160 
TFM04 1 70 


TEM04190 

TEM04200 
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on  noon 


SUbKuUTlNt  *M(  H,  T,  Y,FY,GLB,LUt3,  TERM) 

IMPLICIT  REU*8  (  A-H  ,  G-Z  ) 

DIMENSION  C  n  R ( 2  C ) , Y  DO  T  (20)  »  Y  (  2  0  )  ,  Y  P  (  2  0  )  ,  F  Y  (  2  0 , 5  )  «  P  (  A  )  •  C  (  4  ) 
INTFbER  HLV.DBL 

logical  tlkv 

R  L  A  L  *  R  L Ub  » "AX t  K 

COMMON  /CGEr/PfC,  TbOUND,WR,MPT,\CT,NSTEP,  NEW 
COMMON  /  ANG-M0M  /HSQO 

COKKUN  /PARAM/XJ(4),XJ23,XJ31,XJ21,T1,T2, UMEGAO ,  X P  1  ,  XP 2  ,  X L>  1  ,  X D2  , 
2  X  n  3  v  X  .<  i  v  X  R  2 

COMMuN  /  F  L  L  ^  N  /  T IMDEL  ,  T  I  M  ,  S  I  GM A  ,  I  B  »  KP  ,  MODE R  ,  NR  ,  I  P N P  ,  I  RNk  ,M0L'EP, 

2 IDEL ,MMCDEP, I PNP,XP3,LSE 
DATA  CBL ,HL“/2*0/ 


C 

c 

10 


IRSC^L  =  19.0C+00/2 70.0D+00 
CALL  TORUUEfY) 
call  DER I V( V  f YOO  r ) 

00  20  1=1,  VFC 

20  F  Y (  1,4) =YCUT  (  I  ) 

CALCULATE  PREDKTCR 
DO  30  1*1,  “EC 

30  Y  P (  I  )  =  Y (  I  )  ♦“*( P(  l ) *  F  Y (  I  ,  i)+P(2)*FY(  I,2)+P(3)*FY(  I  ,  3  )  ♦ P C  A ) *F Y (  1,4) 

CALCULATE  CORRE  r T CR 

CALL  OLKIVI Ypf YDOT ) 

on  40  1  =  1,  “FQ 

AC  F  Y (  1,6 ) =  Y  DO  M  I  ) 

DU  6 0  1  =  1,  “LG 

SO  Y (  I  )=Y(  I  ) ♦ H* ( C (  1  )  *  E  Y  {  I  ,2)+C(2)*FY(  I  ,  3  )  +  C (  3  )  *  F  Y  (  I  ,4)+C(4)*FY(  I  »  6  )  ) 
T  =  T  ♦  H 
T  I  M=  I 

N$TEP*NSTEP«  1 
nc  t=nct>mpt 


SEE 


PLOT  f 

go  ro 


WRITE, 

S3 


OR  TERMINATE. 


ss 


IF  IT’S  TIM*  TO 
IF  (NCT.LT. ^6) 

\C  T  =  0 

CALL  OUTPUT ( T , Y , TERM ) 

II  ( TERM)  Gn  TO  130 
\WR* T /  W  R  ♦  I 

IF  (  L>  A  B  S  C  T  -  “  W  R  *  *  R  )  .GT.M  +  2.D0)  GC  TO  5  7 
WR  I  Tl ( 6, 65) T, ( Y(I) , 1*1 , 1 1 ) ,NSTEP,HLV,DBL 
FORMAT  (/»  n  T=*,F7.2/f  Y= • ,  1  1  ( G 1 0 . 3  ,  f , 
l  I  S  ,  *  ,  CoL* • ,  I S/ ) 

X * Y ( 5 ) * ( Y( 6  >  *Y ( 4 ) +  Y( 8  )  ) 

THE  OUT* ( -I./X|)1)*(XU2*Y(4)+XD3*Y(11)-XR1*Y(2)-XR2*Y(10)+X) 
UMSC* (  C  X  J (  l  ) *Y ( 6)  + Y( 6 ) *DCOS( Y ( 4 )  )  )  **2 

1  +  X  J  (  2  )  ♦  Y  (  7)**2 

2  ♦  (  X  J  (  3  )  *  Y ( 8  )  - Y (  5)*0SIN(Y(4)  )  )  *  *  2 


)  /  i 


N  ST  E  P  = 


16, f  ML  V  = 1 


TEM01240 
TEMO 1250 
TEMO  1260 
TEM01270 
TEM012H0 
TE  MO  1 290 
TFM01 300 
TEM01 310 
TEM01 320 
TEMO  1330 


T  F  MO  1 340 
TEMO 1360 
TEMO I  360 
T  EMO  1  3  70 

TEMOi  390 
TEMO  l  400 
TF MO  1  4  10 
TFM01420 
TEMO 1 4  30 
TEM0144C 

)  TEMO  1  ASO 
TEMO  1460 
TEM01 4 70 
T  E  MO  14  80 
TEMO  1490 
TEMO  i  600 
TFMO  1  S  10 
T  E  MO  16  20 
TFMO l S 30 

TEMO  1  640 
TEMO  1660 
TFMO 1 660 
TEMO l 6  70 
TFM01680 
TFM01690 
TEM01600 
TEM01M0 
TEMO  1620 
TEMO 1 630 

,  TEMO  1660 
TEMO  1660 


TFMO  1680 


TEMO  1  700 


3 

~H  SCO ) /“SCO 

TE  MO  1  MO 

WkITt  (6,66)  CM  SC 

TEM01 720 

66 

FORMAT  (•  CHANGE  IN 

SGUARE  UF  ANGULAR  MOMENT U  M  =  f,G10.3) 

TCMO l 730 

67 

IF 

(  T.GC. T  rinUND )  GO 

TL  130 

TEMO  l  740 

c 

TEMO  I  760 

c 

SEE 

I  F 

STEP  SIZ*  SHOULD 

t3fc  CHANGED. 

TEM01760 

OU 

70  1=1,  ‘EC 

TFMO  l  7  70 

IF 

( UAHS ( Y(  I  )  )  .LE  .6 

.0-6)  GU  TU  60 

TEM01 780 
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no  on  noon  on 


COR  t  M*DAGS((Ym-YP(m/Y(m 
GO  TO  70 
60  CGK ( I ) =0 • DO 
70  CONI  I NUE 
M  A  X  =  C  0  R  (  1  ) 

DO  90  1=2,  'EC 
I f  ( MAX-CGR f I ) )  bO, 90, 90 
80  «AX*CUK ( I ) 

90  CONTINUE 

MAX  =  MAX^E^SCAL 
IE  (MAX.LT.OL6)  oC  TO  ICO 
IE  (MAX.LL.I UB )  GO  TO  1 10 

HALVE  H • 

IF  (HPT. EC. ')  GO  TO  110 
h*H/2.00 
MPT  =  M  P  T /? 

HL  V  =  HL  V ♦  I 

CALL  RK(H,T.Y,FY, TERM) 

IE  ( TERM)  Gp  TO  130 

GO  ro  1C 

DOUBLE  H. 

f  EiL  32  IN  ECLL0V  I  No  CARD  A  S  PUT  IN  FOR  THE  MGITAL  SYSTEM  TO  LIMIT 
THF  NUMBER  CF  TIMLS  T  HL  STEP  SUE  COULD  BE  UOULRLED  (WAS  266) 

100  IF  (MPT. EC. V  .OR.  MUD(NCT/MPT,2).NE.0)  GC  TO  110 
H  =  H*2.f)0 
MPT=HPT ♦? 

DHL  =  UHL  ♦  l 

CALL  KK ( H, T , Y • f  Y, TERM ) 

IF  ( TERM)  Gn  TO  130 
GO  10  10 

H  UNCHANGED.  SHIFT  TO  E  I  NO  NEW  SET  CE  DERIVATIVES. 

110  DO  120  J=lf^ 

DO  120  1=1,  NEC 
120  EY(  I  ,  J ) -FY(  I , J+ 1) 

GO  ru  1C 

TERMINATE  INTEGRATION.  WRITE  AND  PUNCH  FINAL  VALUES. 

130  wRITb(6»55)Tt ( Y ( I ), 1=1,11) , N S T E P , HL V , DBL 
C  WK  I  TL  (  7  ,  14°)  T  ,  Y 

140  FORMAT  (716) 

NCT  =  U 
M  P  I  =  1  6 
DDL  -  U 
ML  V  =  0 
RE  I  URN 
FNU 

************************ 


TEM01  790 
TEMO  1800 
TEMO  1810 
T  E  MO  18  20 
TEMO  1830 
TEMO  1840 
TEMO  I860 
TEM01P60 
TFM01870 
TEMO  1880 
TEM01890 
TEMO 1900 
TFM019I0 
TEMO 1 9?0 
TEMO 1930 
TEMO  1940 
TEMO  1950 
TEMO I960 
TEMO  1970 
TEM 01980 
TEM01990 
TEM 02000 
TEM02010 


TFM02020 
TEM 02030 
TEM02040 
TFM02050 
TEM 02 060 
TEM02C70 
T  E MO 2 0  80 
TEM02090 
TFM02  100 
TEMO?  110 
TCM02120 
TEMO?  1  30 
TEM02140 
TEM02  ISO 
TEMO?  160 

TEMO? 180 
TEMO?  190 
TFM02200 
T  F  MO  2  2  10 
TEM 02220 
TEMO??  30 
TEMO  2 240 
TEM 02260 


40 


SUbRGUTI NE  ’K(H, T, Y ,FY, TERM) 

TEM042  10 

I  WPL  IC I f  K  E  “  L  *  8  ( A-h  »  U~  Z  ) 

TFM04220 

01 Mf  NS  I  UN  Y(1),FY(20,5) , YDOTI 20 ) , G 1 ( 20 ) , 02 ( 20) , G3 < 20 1 , G4 ( 20  1  , 

Z  1  (  2  0  T  F  MO  4  2  30 

1  )  ,  Z2(  2C  )  ,  L  *(  20  )  ,PH1  {  20 

TFH, 04240 

LOGICAL  TER" 

T  E MO 4  230 

COMMON  /CG€F/OUM< H) , TBOUND , KR , MPT , NCT , NSTEP ,  NEC 

TEM04260 

COMMON / fcLL  Ev /  T IMOEL, TIM, SIGMA, IB , NP.MOCEK , NR , IRNP,  I RNK ,MODtP 
2 1  DO L  «  MMGDCPi  I PNP , X P 3 , L SE 

» 

TFM042  TO 
TFMO4280 

02=0/2.00 

T  F  MO  4  2  90 

00  bO  M=2 »4 

CALL  TORCUE  (V) 

TEM04  100 

call  ncKivt vf yoct ) 

TFM04  320 

no  io  i=i,  Mec 

TEM04  330 

FV {  I  f  M-  I  ) =  YnOT (  1  ) 

TEM04  140 

G  I  (  I  ) =YOUT (  T  ) 

TFM04  3 bO 

10 

Zl{l)=Y(I)+r'l(l)  *H2 

TFM04360 
TEM04  3  70 

CALL  OCR  1 V ( 7 1 f YOUT ) 

TEM04  380 

OH  20  1*1,  VEC 

TEM04  300 

G 2  (  I  )  =  Y  OL)  T  (  f  ) 

TEM 04400 

?0 

Z2(  IhYllO  '2  (  I  )*H2 

TEM 044 10 
TEM04420 

CALL  DERIVI  72,  YOUT  ) 

TFM044  30 

DO  10  1=1, Mr0 

TEM04440 

G  3  <  I  ) =  YOU  r (  T  ) 

T  E  M04  4  bO 

10 

Z  u  I  )=Y(  1  )  ♦  r  3  (  1  )*H 

TFM04460 
T  F  M  0  4  4  7  0 

CALL  OF R I V (  7  ), YOGT  ) 

TEM 04480 

OU  40  1  =  1,  ‘‘LG 

TCM04490 

G4(  [  ) =  Y DO T (  ?  ) 

T  E  MO  4  bOO 

PHI (  I  )  =  (G1(  T ) ♦ 2 • D0*G2 (  I )+2.D0*G3(l)*G4( I ))/6.D0 

TEM043I0 

40 

Y  {  I  )  =  Y  (  I  ) ♦ H *  PH  l  (  I  ) 

TFM04320 

T  =  T  +  H 

t  im  =  r 

TEM043 30 

\ISTLP  =  NSTfc?*  1 

r  f  m  o  4  s  4  o 

\CT  =  mCT  +MPT 

T  F  M04  b  bO 

I  F  (  MCT.LT.  -*56)  GO  TO  bO 

T  E  MO  4  b  60 

NLJ -0 

TFM04S70 

CALL  OUTPUT ( T , Y , TERM ) 

TFM04380 

II  {  TERM)  RETURN 

TEM04390 

b  0 

CUNT  I NUL 

TEM04600 

K  F  TURN 

T  F  MO  4  7)  1  0 

FNU 

T  F  MO  4  6  20 
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SUBROUTINE  rR2(X,  V) 

01  MENS  I  CM  Xf  I  )  , Y(  1) 

COMMON  /S*ln/  I l RR 

I  N  T  H  Li  L  R  VMPH,HMPHf  VLBL .HLBL  t VCHR ,HCHR,BEOIN 
COMMON  /OK//L»XRtYd,Yl  tVLNtHLN»NPL|N»NPT  »  B  E  G  I  N »  INC 
CATA  CC/2C.7 

CALL  OXOYVI 1 9XLtXRtOX»NXv I  tNNx,OCt  I  ERR ) 

I  F (  IbRR.NE.0)  RETURN 

CALL  OXU Y V (  ^  »  Y6  »  Y  T J,NY  tCC i  ItRR) 

IT  (  I  b  RR  •  NC )  RETURN 

CALL  OR  II)  1  Vf  l  ,  XL  ,  XR  ,  Yb  »  YT  v  UX  9  OY  »  NX  «  M  v  I  «  J  »  NNX  »  NY  ) 

CALL  APLOTV( NPT*!NC , X( BEGIN) • Y ( BEG  I  N  )  ,  I NC  . I NC  t l . 44 ,  !  bR  ) 

DU  1  I  srttoP  *  N  ,  I  NC 

CALL  LINFV(MXV(X(  I  )  )  t  N Y  V ( Y (  I  )  )  »  N  X  V  (  X  (  I  ♦  I NC ))#NYV(Y(I+INC))) 

I  CONTINUE 
RE  I  URN 
END 


TEM03  740 
TFMO  3  750 

T  b  MO  3  7  60 
TEM03770 

T  E  MO  3  7 00 


TCM03810 
TFM038PO 
TFM03R  30 
TEM03H40 
T  E  MO  3850 
TEM03F60 
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SUBROUTINE  UNLOAD  (Y) 

UNL  00010 

C  1 

UNLAGUINo  SIMULATION  LES  0/9 

UNL  00020 

c 

UNL  0 00  30 

c 

COMPUTES  THE  FU'CTIC’N  AScUU  (ISECU) 

UNLOOOAO 

c 

AS E OU  IS  f HI  CpN  T  RCL  LINK  TO 

SUHkOUT I NE  S  TORK  AND  TCKOUt 

UNL00C6O 

c 

UNL  00060 

I  VPL  1  C  I  T  REAL*  8  (  A-H,U-Z  ) 

UNL00070 

real  *r  v ( n 

UNL  0  00  80 

CfIMMuN  /StUU/  DUM  (  3  )  *  I  SECUI  3  ) 

UNL  0  0090 

COMKON  /TOM"/  YAKAX,Y5M1N, 

Y5MAX.Y2MAX, YO SC, MODE 

COMMON/  LLE‘  /DUM1  (  4)  ,NGUER 

,  IDOMI 3)  ( MCDEP 

UNLOO  1  ?0 

c 

UNL001 30 

GL  TU  ( 10, 2^, 10) ,  MOCER 

UNLOO 1 40 

10 

GO  TO  (40,3°),  KGDE 

UNLOO 160 

c 

MODl:*l  NORMAL  OPERATION 

UNLOO 160 

c 

MU0b=2  ROLL  CAC'-UP  CONTROL 

UNLOO I 70 

40 

GO  TO  (50,5^,60,96) ,MGOLP 

6  0 

I  SLUU(  1  ) =0 

UNLOO  190 

V) 

I SE  OU ( 2) =0 

UNL  00  200 

>6 

RETURN 

c 

UNLOO  220 

c 

*************** 

UNL002  30 

c 

UNL00240 

?o  1 3=  i se out  i) 

UNL  00250 

im  3)21  ,21  ,24 

UNLOO  2b0 

2  \ 

IF(GAPS(Y(4)  )  •  L  T  .  Y  A  M  A  X  )  GO 

TG 

AO 

UNL  0  02  7  0 

IE ( Y( 4 ) .GT . VAMAX  )  GO  TU  23 

UNL  0  0  280 

I  S  E  ii  U  (  3)  =  7 

UNL  00290 

GO  TO  4  c 

UNLOO  300 

?  s 

I  SLUG (  3  )  =  3 

UNI  00  3 1 0 

GO  TU  AC 

UNLOO 320 

2A 

1F(  I  1- 3 )  25,2S,26 

UNLOO  330 

26 

IF  ( Y ( 4  )  • L  E  •  0  )  I SbCU(  3 ) =0 

UNL  00  3 AO 

GLI  TO  4  C 

UNLOO  360 

?b 

I  F ( Y ( 4  )  .GE.° )  I SLLU(  3) =0 

UNLOO 3 60 

GO  TO  40 

UNLOO  370 

c 

UNLOO  380 

c 

*********** 

UNLOO  390 

c 

UNLOO  A  00 

30  IF  (  I  SF.QUI  J  ))  31 ,31  ,  J4 

UNL  GOA  10 

31 

IE  (  LARS(  Y(  2  )  )  .L  T.  Y2MAX  )  GIJ 

TO 

40 

UNL  00  A  20 

I K  Y( 2 ) .GE. V2MAX )  GO  10  33 

UNL 00  A  30 

ISLCUI  3  )  =  7 

UNLOO A AO 

uO  TU  4  C 

UNL  0 0  A  60 

3  3 

I SCUUt  3)  =  3 

UNL  00  4  60 

GO  TU  4 C 

UNL  0  0  4  70 

4  4 

IT  (  I  SLQU  (  3  )  -  3  )  36,  36  ,  3b 

UNL  00480 

36 

IF(Y(?).Lt.r')  I  SL  C  U  (  3  )  =G 

UNI  0 0  A  9 0 

GO  TU  40 

UNL  00  500 

36 

I  f  (  Y  (  ?  )  .  GE  .  °  )  1 SfcCUt  3 ) =G 

UNL  00  8  1 0 

GO  T  Li  40 

IJNL00620 

c 

UNL  00  6  30 

C 

********* 

UNL00540 

C 

UNL00550 

5 

0  irt  ISEvJlU  1)  161,61,6? 

UNL  00  660 

6  1 

IF  (  t.AHS  (  Y(  51- YUSU)  .  L  T.DABS  (  Y05U-YDM  I  N)  )GC  TO  90 

UNL  00  6  70 

I  r ( Y (  6 ) .GT . V6MAX )  I  SECU(  I  ) 

=  6 

I  F ( Y (  6  )  • L  T •  Y6MIN)  ISLCUtl 

)  =  1 

1  F  (  1  SLQU (  l  >  .  LG. C 1  WR I  T t ( 6, 

100) Y ( 5) , Y5MIN, Y5MAX, YG50 

UNLOQbOO 
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100 

HJKMAT  (  •  LK^’OKO  THlKC  IS  NO  TKUC  COMBINATION  FCK  YIS)'/' 

Y ( 5 )  =  *  , 6  UNL  00  610 

1  2  0  •  fi  » 1 Y  5  M I 4  = •  ,620*8,  *  Y 5M A X=  •  , E 20. 8  ,  1 

1  Y05G= 1 ,620*8) 

UNL00620 

go  ra  90 

UNL  006  30 

IF  (  ISEQlM  n-l  )  53, 5  3,54 

UNL  00640 

5  1 

I  F  (  Y  (  5  )  -  Yt]  5r  •  L  6 . 0  )  I  S6CL  (  1  )  =0 

UNL 00 6 50 

GO  TO  90 

UNL  00660 

54 

IF( Y( 5)-YC5n*G6.0)  I S6CU( 1 ) =0 

UNL  00  6  70 

GlJ  TO  90 

UNL  006  8  0 

END 

UNL00690 

************************ 
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noo  nooonoon 


C 

c 


c 


c 

c 

c 

c 

c 

c 

c 


TEM 04630 
TEM04640 

SUBkCIU  T  [  NE  T  URK  (  A  A  ,  Hri  )  TEM046S0 

TEM 04660 

A  A  IS  THt  I  H,?US  TER  LOCATION  COORDINATE  ON  Zl,  BB  IS  THE  LOCATION  TEM04670 

COOK D I  NAT  E  UN  Z2.  THIS  SUBROUTINE  ESTABLISHES  A  TCRCUfc  TEM04680 

LOOK-UP  TABIL  FIRST  BY  FILLING  IN  THE  SIGNS  (♦  OR  -)  OF  THE  TEM04690 

TORQUE  COMPONENT  S  F(iR  EACH  THRUSTER  IN  A  12  X  3  ARRAY.  IT  TEN04  700 

THEN  COMPUTES  THE  MAGNITUDES  OF  THE  TORCHES  AND  REPLACFS  EACH  TEM04H0 
ENTRY  WITH  THt  TORQUE  COMPONENTS  A  L  CNG  THt  Zl,  Z2,  AND  Z3  AXES*  TEM04720 

TEM04  7  30 


IMPLICI T  RE *L*8 ( A-H,u-Z ) 

INTEGER*'*  TRUST  1  ,  TRUST2  ,  TRUS  T3  ,  TRUST  4  ,  J 
R  F  A l  *  H  TABLT (  1  ?  ,  3 ) , L  1  , L2 ( 2 )  ,  L3(2),  AA(2),  T(3),  AF(2) 

COMMON  /CCNTRL/  T 

DATA  TAHLt/”l.*l.f~l.»  1  • , 1 • i ~ 1  •  *  1  .  ,  ~ i.,0.,C.,0.,0., 

^  —  1.*  —  1  «  ,  1  •  ,  ~  I.,i.,l.,”l.,C.,0.,0»,0., 

C  — l.f”l.,l.,l.,  —  l.,“"i*  fl.fi.fi., ”'l.t"'l*,l.  /  f 

C  f HET A, PH  I , PI / 30. , 5. , 3. 14 16926 5 3S 897/ ,FCRCE/ .0000  36/ 

CONVERT  F  KUV  DEGREES  TO  RADIANS  AND  F^OM  INCHES  TO  FttT 

THCTR  -  THETA  *  (PI/180,) 

PHK  =  PHT  *  (PI /18C.  ) 

AH  (  1  )  =  A  A (  M / 1 2 . 00*00 

AF ( 2  )  =  AA(  2 ) / 1 2 . OD ♦ UO 

Bf  =  HB/12.nD*00 

LI  =  BF  *  FORCE  *  DSIN(ThFTR) 

DO  8  1=1,2 

L  2  (  l  )  =  AH  M*FlirlCF*DSIMTHETK) 

8  L  3  (I  )  =  AF (  f  ) *FURCE*DCUS( THETR ) 

BFSIN  =  °  F  *  FORCE  *  DSIN(PHR) 

J  =  1 

[ J  )  1=1,8 

TABLET!,  L)  =  DS I CN I L 1 « T ABLE  (  I  ,  I)) 

TABLUI,  2)  =  D  S  I  GN  (  L  2  (  J  )  ,  TABLET  1  ,  2)) 
lAtLHl,  3)  =  DS1GNTL3TJ),  TAbLLU,  3)) 

J  =  J  ♦  1 

I  F (  J  .GE.  1  )  J  =  1 

9  CUNT  I  NU E 

TABLLC9,3)  *  BFSIN 
TAPLE  (  10,  3)  =  -BFSIN 

TARLE (11,3)  =  -BFSIN 
TABLtf  12,3)  =  BFSIN 

WRITE(6,10°)((  TABLE (  I,J),J=l,3),I  =  l,12) 

100  FOKMATT f IT^KCUC  TABLE  IS  SIT  UP  AS  FCLLCWSO1//, 

C  12(lOX,D12.6,10Xtni2.5,10X,ni2.5/)) 

R  t  T  U  <  N 


TE MO 4  74  0 
TEM04750 
T  E  M  0  4  760 
T  E  MO  4  7  70 
TFM04  780 
TEMD4790 
T  E  *04  800 
TEM 04810 
T  E  MO  4  8  20 
TEM04R30 
TEM04840 
TEU04850 
T  E MO  4  8  60 
TEM04H70 
TEM04880 
TEM 04890 
TEM04900 
TFM04010 
TFM04920 
TFM04930 
TEM04940 
TEMDA  960 
TEM04960 
TEM049  1 0 
TCM 04980 
TEM 04990 
TE MO  8 COO 
TEM05C  10 

tf  vosn/o 
T  C  MO  50  30 
TEM05C40 
T  CMOS  060 
TEM05C60 
TFM06070 
TEM06080 
TEM06C90 
TEM06 100 


ENTRY  ThkUSt(  TRUST1,  TkUST?* TRUST  3,TRUST4) 


TEM06  1  10 
TEM05 120 
TFM0S1 30 
TEM06140 

THIS  IS  THt  A r  T I  V  t  PART  UF  THl  SUHkOUTINL,  TRUSTi-4  ARE  TFM0S1S0 

NUN-NFGAT  (  VC  INTEGERS  *H1CH  SIGNIFY  WHICH  AND  HOW  MANY  THRUSTERS  A*  E  TEMOF 1 60 
TU  BE  TURNED  r'N.  IRUSTL  IS  ASSUMED  TC  PE  ZERC  IF  NC  THRUSTERS  ARF  TE*05170 
TU  Ht  FIRED.  THt  SUBROUTINE  ACDS  THE  COMPONENT  TURCUES  OT  THL 
THRUSTERS  FIRHj  TU  THt  TOTAL  TORQUE  CCMPENLNT  S  STGREC  IN  COMMON. 


I  F ( T  R U S  T 1  . L  E .  0)  GO  TO  Lb 


TEM 06  180 

TEM05  190 
TEN*  06200 
TFM062 10 
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ASSIGN  M  TO  J 
K  =  TRUrTl 

CG  TO  L  a 

11  I  F  (  TRUSTS  .LF.  0)  GD  TCJ  18 

ASSIGN  12  TC  J 
K  =  TRUrT2 
GO  TO  l  A 

12  IFITRUSn  .  LE «,  0)  GO  TO  18 

ASSIGN  1 3  TG  J 
K  =  T  R  U  c  T  3 
GG  TU  14 

13  I  F ( TRUST  A  • L  E .  0 )  GO  TO  l H 

ASSIGN  1 8  TO  J 
K  =  TRUC  T  A 
l A  DU  IS  1*1  ,3 

T  (  I  )  =  T  A  8  L  C  (  K  ,  1  )  ♦  TU) 

15  CuNTINUL 

GO  TC  J , ( l 1 , 12, 1 3, 18) 

16  fcR! T£(6, 1 7) 

17  FORMAT ( *  ENTRY  POINT  THRUST  CALLED  UN NtC C SS AR I L Y . ■ ) 

Iti  CONTINUE 
Kt  T  UKN 

END 


TEM0S220 
TFM05230 
TFM05240 
TFM05250 
T  E  MO  8  260 
TFM05270 
TEM05280 
TEM05290 
TEM 05300 
TEM 05310 
T  F  MO  5  320 
TEM05330 
T  E  MO  5  34  0 
TEM05350 
T  E MO  5  360 
T  E  MO  5  3  70 
TEN 05380 
TEM05  390 
TEM0S400 
TEM054  10 
TFM05420 
TEM 08430 
TEM05440 
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Ci  o  o  o  n  n  o  o  n  n  o  r>  n  o  o  non 


SUBROUTINE  TQRQUC  (Y) 
kFAL*H  Y (  I  ) 

<  t  A  L  *8  TTIMr,  h,  TLIMIT,  A T I  ME  ,  A|_  I  M  T  ,  L  (  3  > 

I  N  T  E  G  E  R  *  4  A  c  E  G  U  (  3  )  ,  AXIS,  TCUR,  APQINT,  SCLECH8,  4) 
COMMON  /CCMTRL/  L 

2  /Sfcy"/  H,  ATIMfc,  T  T I VF  ,  ASEQU,  APUINT 

3  / T ABI LS/  SELECT 

DATA  [LIMIT,  ALI^IT  /l.OD+OO,  l. 00+02/ 

SM  TTR^UCS  TC  ZERO 
CALL  UNLOAD  (Y) 

nu  ioc  i-i,3 

100  L ( I )  =  0.00+00 

I F (  APOINT  .GE.  4  )  RETURN 

HAVt  WC  lit  E'1  FIRING  AbOlT  THIS  AXIS  MORE  THAN  ALIMIT 
I F (  AT  IMI  .IT.  ALIMI  T  )  GO  TC  200 

SELECT  NtW  AXIS 

APUINT  ^  A  p°  I  N  T  +  1 

I  F ( A PC  I  NT .  G  r  .  3  )  APOI NT= 1 
AXIS  =  A  S 1 0  U ( APUINT ) 

I F (  (  AXIS  .LT.  1  )  .OR.  (  AXIS  .GT.  8  )  )  RETURN 

AT  IKE  =  O.Or  +00 
TCUR  =  1 

GO  T(J  2  10 


HAVE  WE  fiL  £M  FIRING  THIS  THRUSTER  FCR  MG°E  THAN  TLIMIT 
200  I  F (  TTI*E  .IT.  TLIMIT  )  GG  TO  3C0 
select  new  thruster 

I  CUR  =  ICU<  +  1 

I F (  (TCUR  .rT.  4)  .OR.  ( SE L E C T ( A X I  S  ,  TCUR)  .LT.  I)  )  TCUR 
210  ITIKL  =  $ELrCT ( AXIS,  TCUR) 

TT  IML  =  0. 0^+00 

SET  UP  CALL  TC  THRUST  AND  INCREMENT  TIMES 


300  CALL  THRUST (  IF  IRE , 
AT  I M E  =  A  T I  ‘ ' E  +  H 
I  T  I  ML  =  T  I  I  f,c  +  H 
301  RETURN 
END 


0,  0,  0) 


**$#$$$*$$$*$$****$*$*** 


T E  HO  S4  60 
TF  MO  54  70 
TEM05480 
TEM05490 
TEM05500 
TEMQ55IO 
TFM05520 
TEM05830 
TE  MO  5  540 

TEM, 05550 
T  E  MO  5  560 
TEM05570 
T  E  MO  5  58  0 
T  FM05590 
T  E  MO  5  600 
TFM056I0 
T  E  NO  5  620 
T  E  M0 56  30 
T  E  MO  5640 
TFH05650 

TFM05670 
TEM05680 
TFM05600 
TPM05  700 
TFM05710 
IEM05720 
TEM05  1  30 
TFM05  740 
TEM05  750 
T  F  MO  5 760 
TEM05  7  70 
TE MO 5  7  80 
TEM057  H) 
TFM05800 
TEM058 10 
TEM05820 
T  EMO  6R  30 
TFM05840 
TEM05H50 
TE  MO  5  860 
TFM05R70 
TEM05880 

T  E  M0 5 900 
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no  o  onoooonoo  o  o 


C 


BLOCK  DATA 

IMPLICIT  REAL*8  CA-HiO-Z) 

RIAL  * 8  K 

REAL *  R  V2t M7 

COMMON  / AL  I  r*N /  M2,  M3 

COMMON  /ELLr  N/  TI  KDEL  .  T  I  M  ,  S  1GM  A  ,  I  B  ,  NP  ♦  MODE:  R  »  NR  *  I  P  NP  *  l  RNK  ,  KQOEP  f 
I  I  DE  L , MMCDE  P  »  IPNP,XP3,LSB 
0  l  ML  ‘MS  I  ONI  P  (  A  )  ,  C  I  A  )  ,  X  J  I  4  ) 

OCOMMUN  /CCEr /P  *C»TBOUND,toR,f  PT ,NCT , NSTEP ,  NLQ 

1  /GR/r'UM  l  3 ) ♦ NPL  ,  IDUMI  3)  ,  INC 

2  /°ARAM/XJ , OLM1  (  3 )  , T 1 ,  T 2 , OM LG AO , X P 1 , XP 2 , X 0 1 , X D 2 , XO 3 , X R 1 , 

3  Xr  2 

3  / S  C  CU /  PUM2I3),  ISECJUl  3)  ,  IPOINT 

A  /TABLES/  ISELI8,  A) 

COMMON/PI TCM/XPA, I DFLb, I NP 

1NTIALIZE  /PAR AM/ 

DATA  XJ  /  120. ,  l  30. ,  20, » . 06b/ 

DATm  XP1,XP'>,XU1,XD2,X03,XRL,XR2/1.,.125,6.,.3,0.,.125,  1.0/ 
INITIALIZE  /CDEF/ 

DATA  wPT,NCt,NSTEP,NlG/16,2*0,12/ 

INITIALIZE  /OR/ 

DATA  NPL,  P’C  /O,  1/ 

INITIALIZE  / SE  QL/ 

DATA  I  PC  I  NT,  ISECU/0,0,0,0/ ,DUM2I3)  ,  DUM2 ( 3) /2*1.0D*03/ 

DATA  CUM  2 ( 2  )  /I. 00  +  03/ 


DATA  T  BOUND /600 • / 

DATA  SIGMA  /O.O/ 

DATA  NR  /2/ 

DATA  MGDCR/'V 
DATA  MOOEP  '2/ 

DATA  MMCDEP  / 2OUO0 0 / 

DATA  l DLL  /7A/ 

DATA  XPA,  IQ^LB,  INP/200. ,  20,8/ 
DATA  timoel  /O.O/ 

DATA  NP  / 3 / 

DATA  M2/0.0/ 

DATA  M3/0.0/ 

DATA  XP3/16./ 

DATA  LSR  /brOCOO/ 

DATA  X P  3 / 6 •  / 

INITIALIZE  /SELECT/ 


DATA  ISLE  /^,  L,  3, 

2  7,  7,  7, 

3  D,  0,  0, 

A  n,  0,  0, 

END 


1 1 ♦  1 ,  2,  2,  9, 

12,  R,  R,  6,  10, 

0,  '0,  0,  0,  0, 

0,  0,  0«  0,  0/ 


TF MO  2 260 
TEM 02270 
TEM 02280 


TEM02290 
TEM02300 
TFM02310 
TFM02320 
TEM023  30 
TEM02  3 A 0 
TEM02380 

TEM02360 
TEM02370 
TEM02  380 


T  E  MO  2  A  20 
T  E  M0  2  A  30 
T  E  M  0  2  A  A  0 

TEM02A60 

TEM02A70 

TEM02A80 

TEM02A90 

TEM02800 

TEM02510 

TEM02820 


TEM02SA0 


TEM02SS0 
T  E  MO 2  560 
TFM02  8  f  0 
TEM02880 
TEM02B90 
TEM02600 
T  E  MO  2  6  10 
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o  o  o  o  o  o  o  n  O  o 


SUbRUUUNC  rKl  (FK|X,Y) 

TFMO 3 100 

DIMENSION  XI  1)  ,Y<  1  ) 

TEM03  1  10 

INTEGER  bt  G  T  N , F  R 

T E  MO  3  120 

COMMON  / GR / YL  ,  XK , Y B  »  Y  T  ,  DX  ,  CY 

,  NPL  f  NDO  ?  N  P  T ,  BEGIN,  INC 

TEM03140 

FIND  UPPLR  AND  tGVvER  GRID  LIMITS 

TEM031S0 

CALL  M I NMAX ( Y , NPL , YB, YT ) 

T  E  MO  3  1  60 

IF  (YB)  10,^0,20 

TEV031  70 

10  YR=1.05*Yb 

TE MO  3  180 

(id  ro  30 

TE  MO  3 190 

20  YH  =  G.9S<-Yb 

T EMO  3200 

3U  CONTINUE 

TFMO 32 10 

IF  (YT)  40,60,50 

TFM03220 

40  YT=0.9S*YT 

T  E  M  0  32  30 

GO  T  U  60 

TEM 03240 

SO  Y  T  =  I  .CS*YT 

TEM03260 

60  CONI  I  NUF 

TEM 03260 
TEM 03270 

BEGIN*  1 

T  E  MO  3  280 

XL  =X (  L  ) 

TFM03290 

DY=(  YT-Yb)  /'«0. 

TEM03300 
TEMO  3310 

IS  MURfc  THAN  CMP  FRAME  DESIRED 

TFM03.320 

IF  (FR.CT.  1)  GO  TO  80 

TEMO  3.330 

ENTIRE  GRAPH  TO  EE  PLOTTED  UN  ONE 

FRAME  . 

TEMO  3  34  0 

\  P  T  =  N  P  L 

T  E  MO  3  3  SO 

X  R  =  X  ( NPL  ) 

TEMO 3  360 

r.X=  (  XR-XL  )  /  . 

TEMO  3370 

M)U-ilCG  IN*  (  *  P  T  -  1  )  *  I  NC-  1 

tpmp  34  in 

CALL  GRMX,v) 

T  E  MO  34  20 

RE  TURN 

TFMO  34  30 
TEMO  3440 

GRAPH  10  BE  PLOTTED  ON  MOKE  THAN 

ONE  FRAME. 

TEM034S0 

HO  I  E  R  L  * F  R - L 

TEMO  3460 

NPLE  R  * ( NPL* 1FR  1  ) /FR 

TEMO  34  70 

IF  (MGO( NPL* IFR1 ,FK) .NE.O)  NPLER=NPLFR+1 

TEM03480 

NPT=NPLFR 

TFMQ  3490 

NM1-^PLFR-1 

TEMO  3600 

DX= ( X ( NPT  )-YL ) /20. 

TEM03S  10 
TEMO  3640 

PLOT  INITIAL  FRAMES. 

TEMO  3560 

DXR*2G.*DX 

TEMO 3660 

XR  =  XL  *DXR 

TEMO  3670 

CO  90  I  —  L»  IrKl 

TEM03680 

I  X*  I 

TEMO  3590 

NDL»*rtFG  I  N  +  (  "P  T-  I  )  *  I  NC-  1 

TEMO 3600 

CALL  G  R  ?  ( X , Y ) 

TFMO  3610 

X  L  -  X  R 

TEMO  3620 

X  R  =  X  R  ♦  C  X  R 

TEE.  03630 

BEGIN=REoIN+NM1 

TEMO  3640 

IF  ( XR  •  G E  •  X ( NPL )  )  GO  TO  100 

T E  MO  3 650 

K)  CONTINUE 

TEMO  3fc60 
TFM036 70 

PLOT  I  INAL  EPAMf  . 

TT  MQ  3680 

100  NPT* JPL- I X*NM1 

TFMO  3690 

NDU=3EG  I  N*  CPT  -  1  )  *  I  NC-  1 

T  E MO  3 700 

CALL  G R 2 ( X  *  v ) 

TFMO 3 7  10 

RE  TURN 

TFMO3720 

f  NO 

TFN03MO 
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SUbKOUTINb  “INMAX  (X,  N,  XMIN,  XMAX 
0  I  MENS  I  ON  XII) 

XMIN  =  X (  1  ) 

XMAX  =  X (  l  ) 

DU  10  I =  2  »  N 

IF  (X(  I  1-XMIN)  1  f 2,2 

1  XMIN  =  X (  I  ) 

2  I F  (  XMAX-Xt  T  )  )  3,  10, 1C 

3  XMAX  =  X (  I  ) 

10  CON  T I NU  F 

Kt Tt  < N 
I:  NO 


)  TFM03870 

TFM03880 
T EMO  3890 
T F  MO  3900 
T  F  MO  39  l  0 
TFM03920 
TEM03930 
T F MO  3940 
TFM039S0 
TEMO  3960 
TFM03970 
TFM03980 
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